From d5798a54027d62e8b44497aeea0bde7a22dd1ec4 Mon Sep 17 00:00:00 2001 From: Kailin Date: Tue, 31 Jan 2023 14:48:01 +0100 Subject: [PATCH] Squashed commit: * Support ROS1 and ROS2 * Separate Fixposition Driver Lib for message parsing * Diverse bugfixes in code and documentation --- .clang-format | 5 + .gitignore | 2 +- fixposition_driver/LICENSE => LICENSE | 0 README.md | 160 +- .../converter/base_converter.hpp | 233 - .../fixposition_driver/converter/odometry.hpp | 145 - .../fixposition_driver/converter/tf.hpp | 54 - fixposition_driver/src/base_converter.cpp | 36 - .../src/fixposition_driver_node.cpp | 30 - fixposition_driver/src/imu.cpp | 50 - fixposition_driver/src/llh.cpp | 61 - fixposition_driver/src/odometry.cpp | 250 - fixposition_driver/src/params.cpp | 88 - fixposition_driver/src/tf.cpp | 63 - fixposition_driver_lib/CMakeLists.txt | 78 + .../Doxyfile | 4 +- fixposition_driver_lib/README.md | 37 + .../fixposition_driver_lib-config.cmake.in | 8 + .../converter/base_converter.hpp | 110 + .../fixposition_driver_lib}/converter/imu.hpp | 41 +- .../fixposition_driver_lib}/converter/llh.hpp | 36 +- .../converter/msg_data.hpp | 115 + .../converter/odometry.hpp | 146 + .../fixposition_driver_lib/converter/tf.hpp | 64 + .../fixposition_driver.hpp | 42 +- .../fixposition_driver_lib}/helper.hpp | 13 +- .../fixposition_driver_lib}/params.hpp | 22 +- .../fixposition_driver_lib}/rawdmi.hpp | 22 +- .../time_conversions.hpp | 47 +- fixposition_driver_lib/package.xml | 26 + .../src/fixposition_driver.cpp | 130 +- .../src/helper.cpp | 11 +- fixposition_driver_lib/src/imu.cpp | 50 + fixposition_driver_lib/src/llh.cpp | 65 + fixposition_driver_lib/src/odometry.cpp | 223 + fixposition_driver_lib/src/tf.cpp | 55 + .../CMakeLists.txt | 64 +- fixposition_driver_ros1/Doxyfile | 2427 + fixposition_driver_ros1/README.md | 93 + .../fixposition_driver_ros1/data_to_ros1.hpp | 87 + .../fixposition_driver_node.hpp | 73 + .../fixposition_driver_ros1/params.hpp | 59 + .../launch/rosconsole.conf | 0 .../launch/serial.launch | 8 +- .../launch/serial.yaml | 0 .../launch/tcp.launch | 8 +- .../launch/tcp.yaml | 0 .../msg/Speed.msg | 9 +- .../msg/VRTK.msg | 10 +- .../package.xml | 5 +- .../rviz/fixposition_driver.rviz | 0 fixposition_driver_ros1/src/data_to_ros1.cpp | 94 + .../src/fixposition_driver_node.cpp | 190 + fixposition_driver_ros1/src/params.cpp | 111 + fixposition_driver_ros2/CMakeLists.txt | 85 + fixposition_driver_ros2/Doxyfile | 2427 + fixposition_driver_ros2/README.md | 94 + .../fixposition_driver_ros2/data_to_ros2.hpp | 119 + .../fixposition_driver_node.hpp | 78 + .../fixposition_driver_ros2/params.hpp | 65 + fixposition_driver_ros2/launch/serial.launch | 11 + fixposition_driver_ros2/launch/serial.yaml | 11 + fixposition_driver_ros2/launch/tcp.launch | 12 + fixposition_driver_ros2/launch/tcp.yaml | 11 + fixposition_driver_ros2/msg/Speed.msg | 18 + fixposition_driver_ros2/msg/VRTK.msg | 24 + fixposition_driver_ros2/package.xml | 27 + .../rviz/fixposition_driver.rviz | 283 + .../rviz/fixposition_driver_ros2.rviz | 253 + fixposition_driver_ros2/src/data_to_ros2.cpp | 98 + .../src/fixposition_driver_node.cpp | 196 + fixposition_driver_ros2/src/params.cpp | 106 + fixposition_odometry_converter/CMakeLists.txt | 4 +- fixposition_odometry_converter/COLCON_IGNORE | 0 fixposition_odometry_converter/README.md | 24 + .../odom_converter.hpp | 13 +- .../fixposition_odometry_converter/params.hpp | 9 +- .../launch/odom_converter.yaml | 1 + .../src/odom_converter.cpp | 20 +- .../src/odom_converter_node.cpp | 2 +- fixposition_odometry_converter/src/params.cpp | 10 +- test/HowToTest.md | 177 + test/TestData_example.png | Bin 0 -> 34455 bytes test/data/vrtk2_output_1.txt | 101076 +++++++++++++++ test/data/vrtk2_output_1.txt.tag | Bin 0 -> 240680 bytes 85 files changed, 109626 insertions(+), 1388 deletions(-) create mode 100644 .clang-format rename fixposition_driver/LICENSE => LICENSE (100%) delete mode 100644 fixposition_driver/include/fixposition_driver/converter/base_converter.hpp delete mode 100644 fixposition_driver/include/fixposition_driver/converter/odometry.hpp delete mode 100644 fixposition_driver/include/fixposition_driver/converter/tf.hpp delete mode 100644 fixposition_driver/src/base_converter.cpp delete mode 100644 fixposition_driver/src/fixposition_driver_node.cpp delete mode 100644 fixposition_driver/src/imu.cpp delete mode 100644 fixposition_driver/src/llh.cpp delete mode 100644 fixposition_driver/src/odometry.cpp delete mode 100644 fixposition_driver/src/params.cpp delete mode 100644 fixposition_driver/src/tf.cpp create mode 100644 fixposition_driver_lib/CMakeLists.txt rename {fixposition_driver => fixposition_driver_lib}/Doxyfile (99%) create mode 100644 fixposition_driver_lib/README.md create mode 100644 fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/base_converter.hpp rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/converter/imu.hpp (52%) rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/converter/llh.hpp (52%) create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/msg_data.hpp create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/odometry.hpp create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/tf.hpp rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/fixposition_driver.hpp (67%) rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/helper.hpp (77%) rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/params.hpp (62%) rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/rawdmi.hpp (76%) rename {fixposition_driver/include/fixposition_driver => fixposition_driver_lib/include/fixposition_driver_lib}/time_conversions.hpp (82%) create mode 100644 fixposition_driver_lib/package.xml rename {fixposition_driver => fixposition_driver_lib}/src/fixposition_driver.cpp (62%) rename {fixposition_driver => fixposition_driver_lib}/src/helper.cpp (91%) create mode 100644 fixposition_driver_lib/src/imu.cpp create mode 100644 fixposition_driver_lib/src/llh.cpp create mode 100644 fixposition_driver_lib/src/odometry.cpp create mode 100644 fixposition_driver_lib/src/tf.cpp rename {fixposition_driver => fixposition_driver_ros1}/CMakeLists.txt (62%) create mode 100644 fixposition_driver_ros1/Doxyfile create mode 100644 fixposition_driver_ros1/README.md create mode 100644 fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp create mode 100644 fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp create mode 100644 fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp rename {fixposition_driver => fixposition_driver_ros1}/launch/rosconsole.conf (100%) rename {fixposition_driver => fixposition_driver_ros1}/launch/serial.launch (60%) rename {fixposition_driver => fixposition_driver_ros1}/launch/serial.yaml (100%) rename {fixposition_driver => fixposition_driver_ros1}/launch/tcp.launch (60%) rename {fixposition_driver => fixposition_driver_ros1}/launch/tcp.yaml (100%) rename {fixposition_driver => fixposition_driver_ros1}/msg/Speed.msg (66%) rename {fixposition_driver => fixposition_driver_ros1}/msg/VRTK.msg (78%) rename {fixposition_driver => fixposition_driver_ros1}/package.xml (83%) rename {fixposition_driver => fixposition_driver_ros1}/rviz/fixposition_driver.rviz (100%) create mode 100644 fixposition_driver_ros1/src/data_to_ros1.cpp create mode 100644 fixposition_driver_ros1/src/fixposition_driver_node.cpp create mode 100644 fixposition_driver_ros1/src/params.cpp create mode 100644 fixposition_driver_ros2/CMakeLists.txt create mode 100644 fixposition_driver_ros2/Doxyfile create mode 100644 fixposition_driver_ros2/README.md create mode 100644 fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp create mode 100644 fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp create mode 100644 fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp create mode 100644 fixposition_driver_ros2/launch/serial.launch create mode 100644 fixposition_driver_ros2/launch/serial.yaml create mode 100644 fixposition_driver_ros2/launch/tcp.launch create mode 100644 fixposition_driver_ros2/launch/tcp.yaml create mode 100644 fixposition_driver_ros2/msg/Speed.msg create mode 100644 fixposition_driver_ros2/msg/VRTK.msg create mode 100644 fixposition_driver_ros2/package.xml create mode 100644 fixposition_driver_ros2/rviz/fixposition_driver.rviz create mode 100644 fixposition_driver_ros2/rviz/fixposition_driver_ros2.rviz create mode 100644 fixposition_driver_ros2/src/data_to_ros2.cpp create mode 100644 fixposition_driver_ros2/src/fixposition_driver_node.cpp create mode 100644 fixposition_driver_ros2/src/params.cpp create mode 100644 fixposition_odometry_converter/COLCON_IGNORE create mode 100644 fixposition_odometry_converter/README.md create mode 100644 test/HowToTest.md create mode 100644 test/TestData_example.png create mode 100644 test/data/vrtk2_output_1.txt create mode 100644 test/data/vrtk2_output_1.txt.tag diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..0b202c4 --- /dev/null +++ b/.clang-format @@ -0,0 +1,5 @@ +BasedOnStyle: Google +IndentWidth: 4 +ColumnLimit: 120 +DerivePointerAlignment: false +PointerAlignment: Left diff --git a/.gitignore b/.gitignore index 0481b36..92a8124 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ doc_gen/* -*.vscode* \ No newline at end of file +*.vscode* diff --git a/fixposition_driver/LICENSE b/LICENSE similarity index 100% rename from fixposition_driver/LICENSE rename to LICENSE diff --git a/README.md b/README.md index b9aaf1f..d997322 100644 --- a/README.md +++ b/README.md @@ -1,66 +1,18 @@ -# Fixposition Driver +# Fixposition ROS Driver -[ROS](https://www.ros.org/) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product) +[ROS](https://www.ros.org/) (both ROS1 and ROS2) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product). -## Dependencies +The driver is designed to listen on a TCP or Serial port for the [_Fixposition ASCII Messages_](#fixposition-ascii-messages), and then publish them as corresponding ROS messages. At the same time, the driver can also subscribe to a speed input message, which will be sent back to the Vision-RTK 2 sensor and provide an external speed input. -- [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page), tested with version [3.3.7](https://gitlab.com/libeigen/eigen/-/releases/3.3.7) -- [Boost](https://www.boost.org/), tested with version [1.65.0](https://www.boost.org/users/history/version_1_65_0.html) -- [CMake](https://cmake.org/) -- [Catkin](http://wiki.ros.org/catkin) +- For the output ROS messages, see [Output of the driver](#output-of-the-driver) +- For the input ROS messages for speed input, see [Input Wheelspeed through the driver](#input-wheelspeed-through-the-driver) -- **[fixposition_gnss_tf](https://github.com/fixposition/fixposition_gnss_tf)**: Fixposition GNSS Transformation Lib, minimum version **2.0.0** +## Directory structure +The code is split in the following 3 parts: -This driver operates as a ROS node, connecting to either a TCP or serial stream of Fixposition Vision-RTK output data, see [Fixposition ASCII messages](#fixposition-ascii-messages) and the **Integration Manual**. - -## Installation - -To install the node, extract / clone the code and `fixposition_gnss_tf` to your catkin workspace's `src` folder: - -```bash -# The folder structure should look like this -fp_public_ws -└── src - ├── fixposition_driver - └── gnsstransformationlib -``` - -and build it with: - -`catkin build fixposition_driver` - -Then source your development environment: - -`source devel/setup.bash` - -## Launch the Driver - -- To launch the node in serial mode, run: - - `roslaunch fixposition_driver serial.launch` - -- In TCP mode (Wi-Fi): - - `roslaunch fixposition_driver tcp.launch` - -- In TCP mode (Ethernet): - - `roslaunch fixposition_driver tcp.launch` - -To change the settings of TCP (IP, Port) or Serial (Baudrate, Port) connections, check the `launch/tcp.yaml` and `launch/serial.yaml` files. - -## Input Wheelspeed through the driver - -The fp_ros_driver support inputing a Speed msg (`msg/Speed.msg`) through the `/fixposition/speed` topic. - -The input velocity values should be in [mm/s] as integer 32bit. There are 2 Options: - -- Option 1: only one vehicle speed, then only fill a single value as the vehicle speed -- Option 2: Fill in 4 Values of 4 wheels, in the order of FR, FL, RR, RL - -The input values will be converted into a RAWDMI message and sent via the TCP interface to the Vision-RTK2, where it will be further processed and added into the positioning engine. - -Note: _Currently the wheelspeed input through the ROS driver is only supported in the TCP mode_ +- `fixposition_driver_lib`: common CMake library to parse [_Fixposition ASCII Messages_](#fixposition-ascii-messages). For more details and build instructions, see [here](fixposition_driver_lib/README.md). +- `fixposition_driver_ros1`: ROS1 driver node to subscribe and publish in the ROS1 framework. For more details and build instructions, see [here](fixposition_driver_ros1/README.md). +- `fixposition_driver_ros2`: ROS2 driver node to subscribe and publish in the ROS2 framework. For more details and build instructions, see [here](fixposition_driver_ros2/README.md). ## Output of the driver @@ -71,14 +23,14 @@ The output is published on the following: - From ODOMETRY, at the configured frequency - Messages - | Topic | Message Type | Frequency | Description | - | --------------------------- | ------------------------- | ------------------------------ | ----------------------------------------------------------------------------------------------------- | - | `/fixposition/odometry` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI | - | `/fixposition/odometry_enu` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to ENU0, Velocity and Angular Velocity in FP_POI | - | `/fixposition/ypr` | `geometry_msgs/Vector3` | as configured on web-interface | x = Yaw, y = Pitch, z = Roll in radian. Euler angles representation of rotation between ENU and P_POI. Only available after fusion initialization. | + | Topic | Message Type | Frequency | Description | + | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | + | `/fixposition/odometry` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI | + | `/fixposition/odometry_enu` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI | + | `/fixposition/ypr` | `geometry_msgs/Vector3` | as configured on web-interface | x = Yaw, y = Pitch, z = Roll in radian. Euler angles representation of rotation between ENU and P_POI. Only available after fusion initialization. | | `/fixposition/imu_ypr` | `geometry_msgs/Vector3` | 200Hz | x = 0.0, y = Pitch, z = Roll in radian. Euler angles representation of rotation between a local horizontal frame and P_POI. Rough estimation using IMU alone. | - | `/fixposition/vrtk` | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags | - | `/fixposition/poiimu` | `sensor_msgs/Imu` | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI | + | `/fixposition/vrtk` | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags | + | `/fixposition/poiimu` | `sensor_msgs/Imu` | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI | - From LLH, at the configured frequency @@ -103,7 +55,7 @@ The output is published on the following: - TFs: | Frames | Topic | Message needed to be selected on web-interface | Frequency | - | ------------------ | ------------ | ---------------------------------------------- | ------------------------------ | + | ---------------------------- | ------------ | ---------------------------------------------- | ------------------------------ | | `ECEF-->FP_POI` | `/tf` | `ODOMETRY` | as configured on web-interface | | `ECEF-->FP_ENU` | `/tf` | `ODOMETRY` | as configured on web-interface | | `ECEF-->FP_ENU0` | `/tf` | `ODOMETRY` | as configured on web-interface | @@ -126,15 +78,61 @@ _Please note that the corresponding messages also has to be selected on the Fixp ### Explaination of frame ids -| Frame ID | Explaination | -| ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | -| **ECEF** | Earth-Center-Earth-Fixed frame. | -| **FP_VRTK** | The coordinate frame on the V-RTK's housing on the Fixposition-Logo "X". | -| **FP_POI** | Point-Of-Interest, configured from V-RTK's web-interface with respect to the FP_VRTK frame. By default it is the same as FP_VRTK. | -| **FP_ENU** | The **local** East-North-Up coordinate frame with the origin at the same location as FP_POI. | -| **FP_ENU0** | The **global fixed** East-North-Up coordinate frame with the origin at the first received ODOMETRY position. Needed for visualization in Rviz. | -| **FP_CAM** | The camera coordinate frame of the V-RTK. | -| **FP_IMU_HORIZONTAL** | A local horizontal frame with the origin at the same location as FP_POI. This frame is a rough estimate determined by the IMU alone. | +| Frame ID | Explaination | +| --------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------- | +| **ECEF** | Earth-Center-Earth-Fixed frame. | +| **FP_VRTK** | The coordinate frame on the V-RTK's housing on the Fixposition-Logo "X". | +| **FP_POI** | Point-Of-Interest, configured from V-RTK's web-interface with respect to the FP_VRTK frame. By default it is the same as FP_VRTK. | +| **FP_ENU** | The **local** East-North-Up coordinate frame with the origin at the same location as FP_POI. | +| **FP_ENU0** | The **global fixed** East-North-Up coordinate frame with the origin at the first received ODOMETRY position. Needed for visualization in Rviz. | +| **FP_CAM** | The camera coordinate frame of the V-RTK. | +| **FP_IMU_HORIZONTAL** | A local horizontal frame with the origin at the same location as FP_POI. This frame is a rough estimate determined by the IMU alone. | + +## Input Wheelspeed through the driver + +The fp_ros_driver support inputing a Speed msg (`msg/Speed.msg`) through the `/fixposition/speed` topic. + +The input velocity values should be in [mm/s], respectively [mrad/s], as integer 32bit. There are 2 Options: + +- Option 1: Only one vehicle speed, then only fill a single value as the vehicle speed +- Option 2: One vehicle speed and the rotation around the vehicle's rotation center +- Option 3: Fill in 4 Values of 4 wheels, in the order of FR, FL, RR, RL + +The input values will be converted into a NOV_B-RAWDMI message and sent via the TCP interface to the Vision-RTK2, where it will be further processed and added into the positioning engine. The following protocol is used when filling the DMI messages, as per the documentation: + +| # | Offset | Field | Type | Unit | Example | Description | +| ---: | -----: | ------------- | -------------- | ---- | ------- | ------------------------------------------------------- | +| - | 0 | `sync1` | uint8_t | - | `0xaa` | Sync byte 1 (always `0xaa`) | +| - | 1 | `sync2` | uint8_t | - | `0x44` | Sync byte 2 (always `0x44`) | +| - | 2 | `sync3` | uint8_t | - | `0x13` | Sync byte 3 (always `0x13`) | +| - | 3 | `payload_len` | uint8_t | - | `20` | Payload length (always `20` for this message) | +| - | 4 | `msg_id` | uint16_t | - | `2269` | Message ID (always `2269` for this message) | +| 1 | 6 | `gps_wno` | uint16_t | - | `0` | Week number, set to `0`, not supported by VRTK2 | +| 2 | 8 | `gps_tow` | int32_t | ms | `0` | Time of week [ms], set to `0`, no supported by VRTK2 | +| 3 | 12 | `dmi1` | int32_t | - | | Measurement value 1, for RC or FR wheel | +| 4 | 16 | `dmi2` | int32_t | - | | Measurement value 2, for FL wheel or YW sensor | +| 5 | 20 | `dmi3` | int32_t | - | | Measurement value 3, for RR wheel | +| 6 | 24 | `dmi4` | int32_t | - | | Measurement value 4, for RL wheel | +| - | 28 | `mask` | uint32_t | - | | *Bitfield:* | +| 7 | | `dmi1_valid` | - *bit 0* | - | | Validity flag for *dmi1* value (0 = invalid, 1 = valid) | +| 8 | | `dmi2_valid` | - *bit 1* | - | | Validity flag for *dmi2* value (0 = invalid, 1 = valid) | +| 9 | | `dmi3_valid` | - *bit 2* | - | | Validity flag for *dmi3* value (0 = invalid, 1 = valid) | +| 10 | | `dmi4_valid` | - *bit 3* | - | | Validity flag for *dmi4* value (0 = invalid, 1 = valid) | +| 11 | | `dmi1_type` | - *bits 10…4* | - | | Type of measurement present in *dmi1* value (see below) | +| 12 | | `dmi2_type` | - *bits 17…11* | - | | Type of measurement present in *dmi2* value (see below) | +| 13 | | `dmi3_type` | - *bits 24…18* | - | | Type of measurement present in *dmi3* value (see below) | +| 14 | | `dmi4_type` | - *bits 31…25* | - | | Type of measurement present in *dmi3* value (see below) | +| - | 32 | `checksum` | uint32_t | - | | CRC32 checksum (see VRTK2 documentation) | + +Measurement types (`dmi1_type`, `dmi2_type`, `dmi3_type` and `dmi4_type`): + +| Value | Description | +| :---: | ----------------------- | +| `0` | Linear velocity (speed) | +| `1` | Angular velocity | + +Note: _Currently the wheelspeed input through the ROS driver is only supported in the TCP mode_ + ## Code Documentation @@ -405,21 +403,13 @@ Message fields: | 10 | `orientation_y` | Float (.6) | - | `0.511098` | Quaternion with respect to ECEF, Y component | | 11 | `orientation_z` | Float (.6) | - | `-0.494440` | Quaternion with respect to ECEF, Z component | -# Fixposition Odometry Converter - -An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `Twist`, `TwistWithCov` and `Odometry` are accepted. The x component of the velocity in the input messages is then extracted, converted, and republished to the `/fixposition/speed` topic, where they will be consumed by the VRTK2. -_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one value, i.e. the total vehicle speed. It also assumes that the x axis of the odometry output and the VRTK2 axis are aligned. For situations where all 4 inputs are desired, a custom converter is necessary._ - -## Input Parameters +# Fixposition Odometry Converter -The `odom_converter.yaml` file exposes the necessary parameters for the correct operation of the node. The parameter that may cause the most doubt is the `multiplicative_factor`. This should be chosen such that the inputed float velocity value is transformed into milimeters per second, e.g. 1000 for an input that is expressed in meters per second. +This is an extra node is provided to help with the integration of the wheel odometry on your vehicle. For details, see the subfolder [fixposition_odometry_converter](fixposition_odometry_converter/README.md). -## Launch -After the configuration is set, to launch the node simply run: - `roslaunch fixposition_odometry_converter odom_converter.launch` # License diff --git a/fixposition_driver/include/fixposition_driver/converter/base_converter.hpp b/fixposition_driver/include/fixposition_driver/converter/base_converter.hpp deleted file mode 100644 index 63aca49..0000000 --- a/fixposition_driver/include/fixposition_driver/converter/base_converter.hpp +++ /dev/null @@ -1,233 +0,0 @@ -/** - * @file - * @brief Base Converter to define the interfaces - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -#ifndef __FIXPOSITION_DRIVER_CONVERTER_BASE_CONVERTER__ -#define __FIXPOSITION_DRIVER_CONVERTER_BASE_CONVERTER__ - -/* SYSTEM / STL */ -#include - -/* EXTERNAL */ -#include - -/* ROS */ -#include -#include -#include -#include - -/* PACKAGE */ -#include - -namespace fixposition { -class BaseConverter { - public: - BaseConverter() = default; - ~BaseConverter() = default; - - /** - * @brief API Interface of all converters, to convert the Input string into Ros messages and publish them - * - * @param[in] in_string string of comma delimited message to convert - */ - void ConvertStringAndPublish(const std::string &in_string); - - /** - * @brief Virtual interface to convert the split tokens into ros messages - * - * @param[in] tokens vector of strings split by comma - */ - virtual void ConvertTokensAndPublish(const std::vector &tokens) = 0; - - /** - * @brief Check the header and version of the corresponding message to make sure it is correct - * - * @param[in] msg_header expected header - * @param[in] msg_version expected version - * @return true correct - * @return false incorrect - */ - virtual bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) = 0; -}; - -/** - * @brief Helper function to convert string into double. If string is empty then 0.0 is returned - * - * @param[in] in_str - * @return double - */ -inline double StringToDouble(const std::string &in_str) { return in_str.empty() ? 0. : std::stod(in_str); } - -/** - * @brief Make sure the quaternion is unit quaternion - * - * @param[in] quat geometry_msgs::Quaternion to be checked - * @return true valid quaternion - * @return false invalid quaternion - */ -inline bool CheckQuat(const geometry_msgs::Quaternion &quat) { - return abs(quat.w * quat.w + quat.x * quat.x + quat.y * quat.y + quat.z * quat.z - 1.0) <= 1e-3; -} - -/** - * @brief convert 3 string values into a geometry_msgs::Vector3 - * - * @param[in] x - * @param[in] y - * @param[in] z - * @return geometry_msgs::Vector3 - */ -inline geometry_msgs::Vector3 Vector3ToMsg(const std::string &x, const std::string &y, const std::string &z) { - geometry_msgs::Vector3 msg; - msg.x = StringToDouble(x); - msg.y = StringToDouble(y); - msg.z = StringToDouble(z); - return msg; -} - -/** - * @brief convert 3 string values into a geometry_msgs::Point - * - * @param[in] x - * @param[in] y - * @param[in] z - * @return geometry_msgs::Point - */ -inline geometry_msgs::Point Vector3ToPointMsg(const std::string &x, const std::string &y, const std::string &z) { - geometry_msgs::Point msg; - msg.x = StringToDouble(x); - msg.y = StringToDouble(y); - msg.z = StringToDouble(z); - return msg; -} - -/** - * @brief convert 4 string values into a geometry_msgs::Quaternion - * - * @param[in] w - * @param[in] x - * @param[in] y - * @param[in] z - * @return geometry_msgs::Quaternion - */ -inline geometry_msgs::Quaternion Vector4ToMsg(const std::string &w, const std::string &x, const std::string &y, - const std::string &z) { - geometry_msgs::Quaternion msg; - msg.w = StringToDouble(w); - msg.x = StringToDouble(x); - msg.y = StringToDouble(y); - msg.z = StringToDouble(z); - return msg; -} - -/** - * @brief convert Eigen::Quaterniond into a geometry_msgs::Quaternion - * - * @param[in] quat - * @return geometry_msgs::Quaternion - */ -inline geometry_msgs::Quaternion EigenToQuatMsg(const Eigen::Quaterniond &quat) { - geometry_msgs::Quaternion msg; - msg.w = quat.w(); - msg.x = quat.x(); - msg.y = quat.y(); - msg.z = quat.z(); - return msg; -} - -/** - * @brief convert Eigen::Quaterniond into a geometry_msgs::Quaternion - * - * @param[in] quat - * @return geometry_msgs::Quaternion - */ -inline Eigen::Quaterniond QuatMsgToEigen(const geometry_msgs::Quaternion &msg) { - Eigen::Quaterniond quat; - quat.w() = msg.w; - quat.x() = msg.x; - quat.y() = msg.y; - quat.z() = msg.z; - return quat; -} - -/** - * @brief Convert 3 string values into a Eigen::Vector3d - * - * @param[in] x - * @param[in] y - * @param[in] z - * @return Eigen::Vector3d - */ -inline Eigen::Vector3d Vector3ToEigen(const std::string &x, const std::string &y, const std::string &z) { - return Eigen::Vector3d(StringToDouble(x), StringToDouble(y), StringToDouble(z)); -} - -/** - * @brief Convert Eigen::Vector3d into geometry_msgs::Point - * - * @param[in] v - * @return geometry_msgs::Point - */ -inline geometry_msgs::Point EigenToPointMsg(const Eigen::Vector3d &v) { - geometry_msgs::Point msg; - msg.x = v.x(); - msg.y = v.y(); - msg.z = v.z(); - return msg; -} - -/** - * @brief Convert Eigen::Vector3d into geometry_msgs::Vector3 - * - * @param[in] v - * @return geometry_msgs::Vector3 - */ -inline geometry_msgs::Vector3 EigenToVector3Msg(const Eigen::Vector3d &v) { - geometry_msgs::Vector3 msg; - msg.x = v.x(); - msg.y = v.y(); - msg.z = v.z(); - return msg; -} - -/** - * @brief convert 4 string values into a Eigen::Quaterniond - * - * @param[in] w - * @param[in] x - * @param[in] y - * @param[in] z - * @return Eigen::Quaterniond - */ -inline Eigen::Quaterniond Vector4ToEigen(const std::string &w, const std::string &x, const std::string &y, - const std::string &z) { - return Eigen::Quaterniond(StringToDouble(w), StringToDouble(x), StringToDouble(y), StringToDouble(z)); -} - -/** - * @brief Helper function to convert strings containing gps_wno and gps_tow into ros::Time - * - * @param[in] gps_wno - * @param[in] gps_tow - * @return ros::Time - */ -inline ros::Time ConvertGpsTime(const std::string &gps_wno, const std::string &gps_tow) { - if (!gps_wno.empty() && gps_tow.empty()) { - const times::GpsTime gps_time(std::stoi(gps_wno), std::stod(gps_tow)); - return times::GpsTimeToRosTime(gps_time); - } else { - ROS_DEBUG_STREAM("GPS time empty. Replacing with current ROS time."); - return ros::Time::now(); - } -} -} // namespace fixposition -#endif // __FIXPOSITION_DRIVER_CONVERTER_BASE_CONVERTER__ diff --git a/fixposition_driver/include/fixposition_driver/converter/odometry.hpp b/fixposition_driver/include/fixposition_driver/converter/odometry.hpp deleted file mode 100644 index 5369402..0000000 --- a/fixposition_driver/include/fixposition_driver/converter/odometry.hpp +++ /dev/null @@ -1,145 +0,0 @@ -/** - * @file - * @brief Declaration of OdometryConverter - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -#ifndef __FIXPOSITION_DRIVER_CONVERTER_ODOMETRY__ -#define __FIXPOSITION_DRIVER_CONVERTER_ODOMETRY__ - -/* SYSTEM / STL */ - -/* EXTERNAL */ - -/* ROS */ -#include -#include -#include -#include -#include - -/* PACKAGE */ -#include - -#include -#include - -#include "geometry_msgs/Vector3.h" - -namespace fixposition { - -class OdometryConverter : public BaseConverter { - public: - /** - * @brief Construct a new Fixposition Msg Converter object - * - */ - OdometryConverter(ros::NodeHandle &nh) - : BaseConverter(), - odometry_pub_(nh.advertise("/fixposition/odometry", 100)), - imu_pub_(nh.advertise("/fixposition/poiimu", 100)), - vrtk_pub_(nh.advertise("/fixposition/vrtk", 100)), - odometry_enu0_pub_(nh.advertise("/fixposition/odometry_enu", 100)), - eul_pub_(nh.advertise("/fixposition/ypr", 100)), - eul_imu_pub_(nh.advertise("/fixposition/imu_ypr", 100)), - tf_ecef_enu0_set_(false), - lt_(tf_buffer_) { - tf_ecef_enu0_.header.frame_id = "ECEF"; - tf_ecef_enu0_.child_frame_id = "FP_ENU0"; - } - - ~OdometryConverter() = default; - - bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final { - return msg_header == header_ && std::stoi(msg_version) == kVersion_; - } - - /** - * @brief Comma Delimited FP Output msg - * Example: - * $FP,gpsweek,gps_sec_in_week,x,y,z,qw,qx,qy,qz,vx,vy,vz,omega_x,omega_y,omega_z,ax,ay,az,STATUS,pos_cov(6),orient_cov(6),vel_cov(6),version#XX - * - * @param[in] state state message as string - * @return nav_msgs::Odometry message - */ - virtual void ConvertTokensAndPublish(const std::vector &tokens) final; - - private: - const std::string header_ = "ODOMETRY"; - static constexpr const int kVersion_ = 1; - - ros::Publisher odometry_pub_; //!< ECEF Odometry - ros::Publisher imu_pub_; //!< Bias corrected IMU - ros::Publisher vrtk_pub_; //!< VRTK message - ros::Publisher odometry_enu0_pub_; //!< ENU0 Odometry - ros::Publisher eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU - ros::Publisher eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal frame - - //! transform between ECEF and ENU0 - bool tf_ecef_enu0_set_; //!< flag to indicate if the tf is already set - Eigen::Vector3d t_ecef_enu0_; - Eigen::Quaterniond q_ecef_enu0_; - geometry_msgs::TransformStamped tf_ecef_enu0_; //!< tf from ECEF to ENU0 - - tf2_ros::Buffer tf_buffer_; //!< tf buffer - tf2_ros::TransformListener lt_; //!< tf listener - tf2_ros::TransformBroadcaster br_; //!< tf broadcaster - tf2_ros::StaticTransformBroadcaster static_br_; //!< tf_static broadcaster, for the ENU0 frame which is the ENU - //!< frame at the first received position -}; - -/** - * @brief Build a 6x6 covariance matrix which is 2 independent 3x3 matrices - * - * [xx, xy, xz, 0, 0, 0, - * xy, yy, yz, 0, 0, 0, - * xz, yz, zz, 0, 0, 0, - * 0, 0, 0, xx1, xy1, xz1, - * 0, 0, 0, xy1, yy1, yz1, - * 0, 0, 0, xz1, yz1, zz1] - * - * @param[in] xx - * @param[in] yy - * @param[in] zz - * @param[in] xy - * @param[in] yz - * @param[in] xz - * @param[in] xx1 - * @param[in] yy1 - * @param[in] zz1 - * @param[in] xy1 - * @param[in] yz1 - * @param[in] xz1 - * @return geometry_msgs::PoseWithCovariance::_covariance_type the 6x6 matrix - */ -inline geometry_msgs::PoseWithCovariance::_covariance_type BuildCovMat6D( - const double xx, const double yy, const double zz, const double xy, const double yz, const double xz, double xx1, - const double yy1, const double zz1, const double xy1, const double yz1, double xz1) { - geometry_msgs::PoseWithCovariance::_covariance_type cov; - // Diagonals - cov[0 + 6 * 0] = xx; // 0 - cov[1 + 6 * 1] = yy; // 7 - cov[2 + 6 * 2] = zz; // 14 - cov[3 + 6 * 3] = xx1; // 21 - cov[4 + 6 * 4] = yy1; // 28 - cov[5 + 6 * 5] = zz1; // 35 - - // Rest of values - cov[1 + 6 * 0] = cov[0 + 6 * 1] = xy; // 1 = 6 - cov[2 + 6 * 1] = cov[1 + 6 * 2] = yz; // 8 = 13 - cov[2 + 6 * 0] = cov[0 + 6 * 2] = xz; // 2 = 12 - cov[4 + 6 * 3] = cov[3 + 6 * 4] = xy1; // 22 = 27 - cov[5 + 6 * 4] = cov[4 + 6 * 5] = yz1; // 29 = 34 - cov[5 + 6 * 3] = cov[3 + 6 * 5] = xz1; // 23 = 33 - - return cov; -} - -} // namespace fixposition -#endif // __FIXPOSITION_DRIVER_CONVERTER_ODOMETRY__ diff --git a/fixposition_driver/include/fixposition_driver/converter/tf.hpp b/fixposition_driver/include/fixposition_driver/converter/tf.hpp deleted file mode 100644 index c943f06..0000000 --- a/fixposition_driver/include/fixposition_driver/converter/tf.hpp +++ /dev/null @@ -1,54 +0,0 @@ -/** - * @file - * @brief Declaration of TfConverter - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -#ifndef __FIXPOSITION_DRIVER_CONVERTER_TF__ -#define __FIXPOSITION_DRIVER_CONVERTER_TF__ - -/* ROS */ -#include -#include - -/* PACKAGE */ -#include - -namespace fixposition { - -class TfConverter : public BaseConverter { - public: - /** - * @brief Construct a new Fixposition Msg Converter object - * - */ - TfConverter(ros::NodeHandle& nh) {} - - ~TfConverter() = default; - - bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final { - return msg_header == header_ && std::stoi(msg_version) == kVersion_; - } - /** - * @brief TODO - * - * @param[in] state state message as string - * @return nav_msgs::Odometry message - */ - void ConvertTokensAndPublish(const std::vector& tokens) final; - - private: - tf2_ros::TransformBroadcaster br_; - tf2_ros::StaticTransformBroadcaster static_br_; - const std::string header_ = "TF"; - - static constexpr const int kVersion_ = 1; -}; -} // namespace fixposition -#endif // __FIXPOSITION_DRIVER_CONVERTER_TF__ diff --git a/fixposition_driver/src/base_converter.cpp b/fixposition_driver/src/base_converter.cpp deleted file mode 100644 index e270a8d..0000000 --- a/fixposition_driver/src/base_converter.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/** - * @file - * @brief Implementation of Base Converter to define the interfaces - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* PACKAGE */ -#include -#include - -namespace fixposition { -void BaseConverter::ConvertStringAndPublish(const std::string& in_string) { - if (!in_string.empty()) { - std::vector tokens; - SplitMessage(tokens, in_string, ","); - - if (CheckHeaderAndVersion(tokens[1], tokens[2])) { - ROS_DEBUG_STREAM("Received " << tokens[1] << "," << tokens[2] << " message."); - ConvertTokensAndPublish(tokens); - } else { - ROS_DEBUG_STREAM("Unknown message! Type: " << tokens[1] << " Version: " << tokens[2]); - return; - } - - } else { - ROS_ERROR_STREAM("State message empty! Not well parsed?"); - } -} - -} // namespace fixposition diff --git a/fixposition_driver/src/fixposition_driver_node.cpp b/fixposition_driver/src/fixposition_driver_node.cpp deleted file mode 100644 index 46a9560..0000000 --- a/fixposition_driver/src/fixposition_driver_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file - * @brief Main function for the fixposition driver ros node - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* ROS */ -#include -#include - -/* PACKAGE */ -#include - -int main(int argc, char **argv) { - ros::init(argc, argv, "fixposition_driver"); - ros::NodeHandle node_handle; - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - fixposition::FixpositionDriver fixposition_out(&node_handle); - ROS_DEBUG("Starting node..."); - fixposition_out.Run(); - ros::waitForShutdown(); - ROS_DEBUG("Exiting."); - return 0; -} diff --git a/fixposition_driver/src/imu.cpp b/fixposition_driver/src/imu.cpp deleted file mode 100644 index b7788c1..0000000 --- a/fixposition_driver/src/imu.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @file - * @brief Implementation of ImuConverter converter - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* PACKAGE */ -#include - -namespace fixposition { - -/// msg field indices -static constexpr const int msg_type_idx = 1; -static constexpr const int msg_version_idx = 2; -static constexpr const int gps_week_idx = 3; -static constexpr const int gps_tow_idx = 4; -static constexpr const int acc_x_idx = 5; -static constexpr const int acc_y_idx = 6; -static constexpr const int acc_z_idx = 7; -static constexpr const int rot_x_idx = 8; -static constexpr const int rot_y_idx = 9; -static constexpr const int rot_z_idx = 10; - -void ImuConverter::ConvertTokensAndPublish(const std::vector& tokens) { - sensor_msgs::Imu msg; - if (tokens.size() != 11) { - ROS_INFO("Error in parsing IMU string with %lu fields! IMU message will be empty.", tokens.size()); - return; - } - // header stamps - msg.header.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); - msg.linear_acceleration.x = StringToDouble(tokens.at(acc_x_idx)); - msg.linear_acceleration.y = StringToDouble(tokens.at(acc_y_idx)); - msg.linear_acceleration.z = StringToDouble(tokens.at(acc_z_idx)); - - msg.angular_velocity.x = StringToDouble(tokens.at(rot_x_idx)); - msg.angular_velocity.y = StringToDouble(tokens.at(rot_y_idx)); - msg.angular_velocity.z = StringToDouble(tokens.at(rot_z_idx)); - if (imu_pub_.getNumSubscribers() > 0) { - imu_pub_.publish(msg); - } -} - -} // namespace fixposition diff --git a/fixposition_driver/src/llh.cpp b/fixposition_driver/src/llh.cpp deleted file mode 100644 index cad9400..0000000 --- a/fixposition_driver/src/llh.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/** - * @file - * @brief Implementation of LlhConverter - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* PACKAGE */ -#include - -namespace fixposition { - -/// msg field indices -static constexpr const int msg_type_idx = 1; -static constexpr const int msg_versio_idx = 2; -static constexpr const int gps_week_idx = 3; -static constexpr const int gps_tow_idx = 4; -static constexpr const int latitude_idx = 5; -static constexpr const int longitude_idx = 6; -static constexpr const int height_idx = 7; -static constexpr const int pos_cov_ee_idx = 8; -static constexpr const int pos_cov_nn_idx = 9; -static constexpr const int pos_cov_uu_idx = 10; -static constexpr const int pos_cov_en_idx = 11; -static constexpr const int pos_cov_nu_idx = 12; -static constexpr const int pos_cov_eu_idx = 13; - -void LlhConverter::ConvertTokensAndPublish(const std::vector& tokens) { - sensor_msgs::NavSatFix navsat_msg; - if (tokens.size() != 14) { - ROS_INFO("Error in parsing LLH string with %lu fields! NavSatFix message will be empty.", tokens.size()); - return; - } - // header stamps - navsat_msg.header.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); - - navsat_msg.header.frame_id = "POI"; - navsat_msg.latitude = StringToDouble(tokens.at(latitude_idx)); - navsat_msg.longitude = StringToDouble(tokens.at(longitude_idx)); - navsat_msg.altitude = StringToDouble(tokens.at(height_idx)); - - // Covariance diagonals - navsat_msg.position_covariance[0] = StringToDouble(tokens.at(pos_cov_ee_idx)); - navsat_msg.position_covariance[4] = StringToDouble(tokens.at(pos_cov_nn_idx)); - navsat_msg.position_covariance[8] = StringToDouble(tokens.at(pos_cov_uu_idx)); - - // Rest of covariance fields - navsat_msg.position_covariance[1] = navsat_msg.position_covariance[3] = StringToDouble(tokens.at(pos_cov_en_idx)); - navsat_msg.position_covariance[2] = navsat_msg.position_covariance[6] = StringToDouble(tokens.at(pos_cov_nu_idx)); - navsat_msg.position_covariance[5] = navsat_msg.position_covariance[7] = StringToDouble(tokens.at(pos_cov_eu_idx)); - navsat_msg.position_covariance_type = 3; - - navsatfix_pub_.publish(navsat_msg); -} - -} // namespace fixposition diff --git a/fixposition_driver/src/odometry.cpp b/fixposition_driver/src/odometry.cpp deleted file mode 100644 index 1124ee3..0000000 --- a/fixposition_driver/src/odometry.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/** - * @file - * @brief Implementation of OdometryConverter - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* EXTERNAL */ -#include -#include - -/* ROS */ -#include - -/* FIXPOSITION */ -#include - -/* PACKAGE */ -#include -#include - -namespace fixposition { - -/// msg field indices -static constexpr const int msg_type_idx = 1; -static constexpr const int msg_version_idx = 2; -static constexpr const int gps_week_idx = 3; -static constexpr const int gps_tow_idx = 4; -static constexpr const int pos_x_idx = 5; -static constexpr const int pos_y_idx = 6; -static constexpr const int pos_z_idx = 7; -static constexpr const int orientation_w_idx = 8; -static constexpr const int orientation_x_idx = 9; -static constexpr const int orientation_y_idx = 10; -static constexpr const int orientation_z_idx = 11; -static constexpr const int vel_x_idx = 12; -static constexpr const int vel_y_idx = 13; -static constexpr const int vel_z_idx = 14; -static constexpr const int rot_x_idx = 15; -static constexpr const int rot_y_idx = 16; -static constexpr const int rot_z_idx = 17; -static constexpr const int acc_x_idx = 18; -static constexpr const int acc_y_idx = 19; -static constexpr const int acc_z_idx = 20; -static constexpr const int fusion_status_idx = 21; -static constexpr const int imu_bias_status_idx = 22; -static constexpr const int gnss_fix_type_idx = 23; -static constexpr const int wheelspeed_status_idx = 24; -static constexpr const int pos_cov_xx_idx = 25; -static constexpr const int pos_cov_yy_idx = 26; -static constexpr const int pos_cov_zz_idx = 27; -static constexpr const int pos_cov_xy_idx = 28; -static constexpr const int pos_cov_yz_idx = 29; -static constexpr const int pos_cov_xz_idx = 30; -static constexpr const int orientation_cov_xx_idx = 31; -static constexpr const int orientation_cov_yy_idx = 32; -static constexpr const int orientation_cov_zz_idx = 33; -static constexpr const int orientation_cov_xy_idx = 34; -static constexpr const int orientation_cov_yz_idx = 35; -static constexpr const int orientation_cov_xz_idx = 36; -static constexpr const int vel_cov_xx_idx = 37; -static constexpr const int vel_cov_yy_idx = 38; -static constexpr const int vel_cov_zz_idx = 39; -static constexpr const int vel_cov_xy_idx = 40; -static constexpr const int vel_cov_yz_idx = 41; -static constexpr const int vel_cov_xz_idx = 42; -static constexpr const int sw_version_idx = 43; - -void OdometryConverter::ConvertTokensAndPublish(const std::vector& tokens) { - if (tokens.size() != 44) { - ROS_INFO("Error in parsing Odometry string with %lu fields! odometry and status messages will be empty.", - tokens.size()); - return; - } - - const int fusion_status = tokens.at(fusion_status_idx).empty() ? 0 : tokens.at(fusion_status_idx)[0] - '0'; - const bool fusion_init = fusion_status >= 3; - - const bool odom_sub_avl = odometry_pub_.getNumSubscribers() > 0; - const bool vrtk_sub_avl = vrtk_pub_.getNumSubscribers() > 0; - const bool imu_sub_avl = imu_pub_.getNumSubscribers() > 0; - const bool eul_sub_avl = eul_pub_.getNumSubscribers() > 0; - const bool eul_imu_sub_avl = eul_imu_pub_.getNumSubscribers() > 0; - const bool odom_enu0_sub_avl = odometry_enu0_pub_.getNumSubscribers() > 0; - - // common data - const auto stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); - const Eigen::Vector3d t_ecef_body = - Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx)); - const Eigen::Quaterniond q_ecef_body = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx), - tokens.at(orientation_y_idx), tokens.at(orientation_z_idx)); - if (fusion_init) { - // TFs - geometry_msgs::TransformStamped tf_ecef_poi, tf_ecef_enu; - tf_ecef_enu.header.stamp = stamp; - tf_ecef_poi.header.stamp = stamp; - tf_ecef_enu.header.frame_id = "ECEF"; - tf_ecef_poi.header.frame_id = "ECEF"; - tf_ecef_poi.child_frame_id = "FP_POI"; - tf_ecef_enu.child_frame_id = "FP_ENU"; // The ENU frame at the position of FP_POI - - // static TF ECEF ENU0 - if (!tf_ecef_enu0_set_ && tf_ecef_enu0_.transform.translation.x == 0 && - tf_ecef_enu0_.transform.translation.y == 0 && tf_ecef_enu0_.transform.translation.z == 0) { - // ENU0 frame is not yet set, set the same ENU tf to ENU0 - t_ecef_enu0_ = t_ecef_body; - q_ecef_enu0_ = Eigen::Quaterniond(gnss_tf::RotEnuEcef(t_ecef_body).transpose()); - tf_ecef_enu0_.transform.translation = EigenToVector3Msg(t_ecef_enu0_); - tf_ecef_enu0_.transform.rotation = EigenToQuatMsg(q_ecef_enu0_); - tf_ecef_enu0_set_ = true; - } - - // TF ECEF POI is basically the same as the odometry, containing the Pose of the POI in the ECEF Frame - tf_ecef_poi.transform.translation = EigenToVector3Msg(t_ecef_body); - tf_ecef_poi.transform.rotation = EigenToQuatMsg(q_ecef_body); - - // Quaternion from ECEF to Local ENU at POI - const Eigen::Quaterniond q_ecef_enu = Eigen::Quaterniond(gnss_tf::RotEnuEcef(t_ecef_body).transpose()); - // TF ECEF ENU - tf_ecef_enu.transform.translation = EigenToVector3Msg(t_ecef_body); - tf_ecef_enu.transform.rotation = EigenToQuatMsg(q_ecef_enu); - - // Send TFs - if (CheckQuat(tf_ecef_poi.transform.rotation)) { - br_.sendTransform(tf_ecef_poi); - } - if (CheckQuat(tf_ecef_enu.transform.rotation)) { - br_.sendTransform(tf_ecef_enu); - } - // Send Static TF ECEF ENU0 - if (tf_ecef_enu0_set_ && CheckQuat(tf_ecef_enu0_.transform.rotation)) { - static_br_.sendTransform(tf_ecef_enu0_); - } - } - // Msgs - nav_msgs::Odometry odom_msg; //!< Odmetry msg ECEF - FP_POI - fixposition_driver::VRTK vrtk_msg; - if (fusion_init) { - odom_msg.header.stamp = stamp; - odom_msg.header.frame_id = "ECEF"; - odom_msg.child_frame_id = "FP_POI"; - - vrtk_msg.header.stamp = stamp; - vrtk_msg.header.frame_id = "ECEF"; - vrtk_msg.pose_frame = "FP_POI"; - vrtk_msg.kin_frame = "FP_POI"; - - // Pose & Cov - odom_msg.pose.pose.position = EigenToPointMsg(t_ecef_body); - odom_msg.pose.pose.orientation = EigenToQuatMsg(q_ecef_body); - odom_msg.pose.covariance = BuildCovMat6D( - StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)), - StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)), - StringToDouble(tokens.at(pos_cov_yz_idx)), StringToDouble(tokens.at(pos_cov_xz_idx)), - StringToDouble(tokens.at(orientation_cov_xx_idx)), StringToDouble(tokens.at(orientation_cov_yy_idx)), - StringToDouble(tokens.at(orientation_cov_zz_idx)), StringToDouble(tokens.at(orientation_cov_xy_idx)), - StringToDouble(tokens.at(orientation_cov_yz_idx)), StringToDouble(tokens.at(orientation_cov_xz_idx))); - vrtk_msg.pose = odom_msg.pose; - - // Twist & Cov - // Linear - odom_msg.twist.twist.linear = Vector3ToMsg(tokens.at(vel_x_idx), tokens.at(vel_y_idx), tokens.at(vel_z_idx)); - // Angular - odom_msg.twist.twist.angular = Vector3ToMsg(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx)); - odom_msg.twist.covariance = BuildCovMat6D( - StringToDouble(tokens.at(vel_cov_xx_idx)), StringToDouble(tokens.at(vel_cov_yy_idx)), - StringToDouble(tokens.at(vel_cov_zz_idx)), StringToDouble(tokens.at(vel_cov_xy_idx)), - StringToDouble(tokens.at(vel_cov_yz_idx)), StringToDouble(tokens.at(vel_cov_xz_idx)), 0, 0, 0, 0, 0, 0); - vrtk_msg.velocity = odom_msg.twist; - } - - // Status, regardless of fusion_init - vrtk_msg.fusion_status = tokens.at(fusion_status_idx).empty() ? 0 : tokens.at(fusion_status_idx)[0] - '0'; - vrtk_msg.imu_bias_status = tokens.at(imu_bias_status_idx).empty() ? 0 : tokens.at(imu_bias_status_idx)[0] - '0'; - vrtk_msg.gnss_status = tokens.at(gnss_fix_type_idx).empty() ? 0 : tokens.at(gnss_fix_type_idx)[0] - '0'; - vrtk_msg.wheelspeed_status = - tokens.at(wheelspeed_status_idx).empty() ? 0 : tokens.at(wheelspeed_status_idx)[0] - '0'; - - vrtk_msg.version = tokens.at(sw_version_idx).empty() ? "UNKNOWN" : tokens.at(sw_version_idx); - - // Send Ros msgs - if (odom_sub_avl && fusion_init) { - odometry_pub_.publish(odom_msg); - } - if (vrtk_sub_avl) { - vrtk_pub_.publish(vrtk_msg); - } - if (imu_sub_avl) { - // POI IMU Message - sensor_msgs::Imu imu_msg; - imu_msg.header.stamp = stamp; - imu_msg.header.frame_id = "FP_POI"; - // Omega - imu_msg.angular_velocity = odom_msg.twist.twist.angular; - // Acceleration - imu_msg.linear_acceleration = Vector3ToMsg(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx)); - imu_pub_.publish(imu_msg); - } - if (eul_sub_avl && fusion_init) { - // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll - const Eigen::Vector3d ypr = gnss_tf::EcefPoseToEnuEul(t_ecef_body, q_ecef_body.toRotationMatrix()); - eul_pub_.publish(EigenToVector3Msg(ypr)); - } - if (eul_imu_sub_avl) { - bool valid = true; - geometry_msgs::TransformStamped imu_pose; - try { - // Check if the transform is being correctly published - imu_pose = tf_buffer_.lookupTransform("FP_IMU_HORIZONTAL", "FP_POI", ros::Time(0)); - } catch (tf2::TransformException &ex) { - valid = false; - } if (valid) { - const Eigen::Quaterniond quat_pose = QuatMsgToEigen(imu_pose.transform.rotation); - Eigen::Vector3d imu_ypr = gnss_tf::QuatToEul(quat_pose); - imu_ypr.x() = 0.0; // the yaw value is not observable using IMU alone - eul_imu_pub_.publish(EigenToVector3Msg(imu_ypr)); - } - } - if (odom_enu0_sub_avl && fusion_init) { - // Odmetry msg ENU0 - FP_POI - nav_msgs::Odometry odom_enu0_msg; - odom_enu0_msg.header.stamp = stamp; - odom_enu0_msg.header.frame_id = "ENU0"; - odom_enu0_msg.child_frame_id = "FP_POI"; - // Pose - // convert position in ECEF into position in ENU0 - const Eigen::Vector3d t_enu0_body = gnss_tf::TfEnuEcef(t_ecef_body, gnss_tf::TfWgs84LlhEcef(t_ecef_enu0_)); - const Eigen::Quaterniond q_enu0_body = q_ecef_enu0_.inverse() * q_ecef_body; - odom_enu0_msg.pose.pose.position = EigenToPointMsg(t_enu0_body); - odom_enu0_msg.pose.pose.orientation = EigenToQuatMsg(q_enu0_body); - // Cov - Eigen::Matrix cov_ecef(odom_msg.pose.covariance.data()); - const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0_.toRotationMatrix(); - Eigen::Map> cov_enu0(odom_enu0_msg.pose.covariance.data()); - cov_enu0.topLeftCorner(3, 3) = rot_ecef_enu0 * cov_ecef.topLeftCorner(3, 3) * rot_ecef_enu0.transpose(); - cov_enu0.bottomRightCorner(3, 3) = rot_ecef_enu0 * cov_ecef.bottomRightCorner(3, 3) * rot_ecef_enu0.transpose(); - - // Twist is the same as it is in the FP_POI frame - odom_enu0_msg.twist = odom_msg.twist; - - // Publish - odometry_enu0_pub_.publish(odom_enu0_msg); - } -} -} // namespace fixposition diff --git a/fixposition_driver/src/params.cpp b/fixposition_driver/src/params.cpp deleted file mode 100644 index aa9acc0..0000000 --- a/fixposition_driver/src/params.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/** - * @file - * @brief Implementation of Parameter Loading - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* ROS */ -#include -#include - -/* PACKAGE */ -#include - -namespace fixposition { - -bool FpOutputParams::LoadFromRos(const std::string &ns) { - // read parameters - if (!ros::param::get(ns + "/rate", rate)) rate = 100; - - if (!ros::param::get(ns + "/reconnect_delay", reconnect_delay)) reconnect_delay = 5.0; - - std::string type_str; - if (!ros::param::get(ns + "/type", type_str)) { - return false; - } - if (type_str == "tcp") { - type = INPUT_TYPE::TCP; - } else if (type_str == "serial") { - type = INPUT_TYPE::SERIAL; - } else { - ROS_ERROR("Input type has to be tcp or serial!"); - return false; - } - - if (!ros::param::get(ns + "/formats", formats)) formats = {"ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"}; - - // Get parameters: port (required) - if (!ros::param::get(ns + "/port", port)) { - } - - if (type == INPUT_TYPE::TCP) { - if (!ros::param::get(ns + "/ip", ip)) { - // default value for IP - ip = "10.0.2.1"; - } - if (!ros::param::get(ns + "/port", port)) { - // default value for TCP port - port = "21000"; - } - } else if (type == INPUT_TYPE::SERIAL) { - if (!ros::param::get(ns + "/baudrate", baudrate)) { - // default value for baudrate - baudrate = 115200; - } - if (!ros::param::get(ns + "/port", port)) { - // default value for serial port - port = "/dev/ttyUSB0"; - } - } - - return true; -} - -bool CustomerInputParams::LoadFromRos(const std::string &ns) { - if (!ros::param::get(ns + "/speed_topic", speed_topic)) { - // default value for the topic name - speed_topic = "/fixposition/speed"; - } - - return true; -} - -bool FixpositionDriverParams::LoadFromRos(const std::string &ns) { - bool ok = true; - - ok &= fp_output.LoadFromRos(ns + "fp_output"); - ok &= customer_input.LoadFromRos(ns + "customer_input"); - - return ok; -} - -} // namespace fixposition diff --git a/fixposition_driver/src/tf.cpp b/fixposition_driver/src/tf.cpp deleted file mode 100644 index a40327f..0000000 --- a/fixposition_driver/src/tf.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/** - * @file - * @brief Implementation of TfConverter - * - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * - */ - -/* ROS */ -#include - -/* PACKAGE */ -#include - -namespace fixposition { - -/// msg field indices -static constexpr const int msg_type_idx = 1; -static constexpr const int msg_version_idx = 2; -static constexpr const int from_frame_idx = 3; -static constexpr const int to_frame_idx = 4; -static constexpr const int translation_x_idx = 5; -static constexpr const int translation_y_idx = 6; -static constexpr const int translation_z_idx = 7; -static constexpr const int orientation_w_idx = 8; -static constexpr const int orientation_x_idx = 9; -static constexpr const int orientation_y_idx = 10; -static constexpr const int orientation_z_idx = 11; - -void TfConverter::ConvertTokensAndPublish(const std::vector& tokens) { - geometry_msgs::TransformStamped tf; - if (tokens.size() != 12) { - ROS_INFO("Error in parsing TF string with %lu fields! TFs will be empty.", tokens.size()); - return; - } - // header stamps - tf.header.stamp = ros::Time::now(); - tf.header.frame_id = "FP_" + tokens.at(from_frame_idx); - tf.child_frame_id = "FP_" + tokens.at(to_frame_idx); - - tf.transform.translation.x = StringToDouble(tokens.at(translation_x_idx)); - tf.transform.translation.y = StringToDouble(tokens.at(translation_y_idx)); - tf.transform.translation.z = StringToDouble(tokens.at(translation_z_idx)); - - tf.transform.rotation.w = StringToDouble(tokens.at(orientation_w_idx)); - tf.transform.rotation.x = StringToDouble(tokens.at(orientation_x_idx)); - tf.transform.rotation.y = StringToDouble(tokens.at(orientation_y_idx)); - tf.transform.rotation.z = StringToDouble(tokens.at(orientation_z_idx)); - - if (CheckQuat(tf.transform.rotation)) { - if (tf.child_frame_id == "FP_IMU_HORIZONTAL") { - br_.sendTransform(tf); - } else { - static_br_.sendTransform(tf); - } - } -} - -} // namespace fixposition diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt new file mode 100644 index 0000000..a4bad92 --- /dev/null +++ b/fixposition_driver_lib/CMakeLists.txt @@ -0,0 +1,78 @@ +# GENERAL ============================================================================================================== +cmake_minimum_required(VERSION 3.5) +project(fixposition_driver_lib VERSION 5.0.0 LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Wall -Wextra -Wpedantic -Wno-unused-parameter") +set(CMAKE_CXX_FLAGS_RELEASE "-O3") +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# DEPENDENCIES ========================================================================================================= +find_package(Boost 1.65.0 REQUIRED) +find_package(Eigen3 REQUIRED) + +find_package(fixposition_gnss_tf REQUIRED) + +include_directories(include + ${fixposition_gnss_tf_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIR} +) + +# BUILD EXECUTABLE ===================================================================================================== +add_library( + ${PROJECT_NAME} SHARED + src/fixposition_driver.cpp + src/odometry.cpp + src/llh.cpp + src/imu.cpp + src/tf.cpp + src/helper.cpp +) + +target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread) + +# INSTALL ============================================================================================================== +list(APPEND PACKAGE_LIBRARIES ${PROJECT_NAME}) + +install( + DIRECTORY include + DESTINATION . +) + +# Mark executables and/or libraries for installation +install(TARGETS ${PACKAGE_LIBRARIES} EXPORT ${PROJECT_NAME}-targets DESTINATION lib) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(EXPORT ${PROJECT_NAME}-targets DESTINATION lib/cmake/${PROJECT_NAME}) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" PATTERN "*.hxx" +) + +install(FILES package.xml DESTINATION share/${PROJECT_NAME}) + +# Create cmake config files +# See doc: https://cmake.org/cmake/help/latest/module/CMakePackageConfigHelpers.html#example-generating-package-files +include(CMakePackageConfigHelpers) +configure_package_config_file(${CMAKE_CURRENT_LIST_DIR}/cmake/${PROJECT_NAME}-config.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake + INSTALL_DESTINATION lib/cmake/${PROJECT_NAME} + NO_CHECK_REQUIRED_COMPONENTS_MACRO) + +write_basic_package_version_file(${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake + VERSION ${PROJECT_VERSION} COMPATIBILITY ExactVersion) + +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake" + DESTINATION lib/cmake/${PROJECT_NAME}) + +export(EXPORT ${PROJECT_NAME}-targets FILE ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-targets.cmake) diff --git a/fixposition_driver/Doxyfile b/fixposition_driver_lib/Doxyfile similarity index 99% rename from fixposition_driver/Doxyfile rename to fixposition_driver_lib/Doxyfile index 694909f..f540bc9 100644 --- a/fixposition_driver/Doxyfile +++ b/fixposition_driver_lib/Doxyfile @@ -32,7 +32,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "Fixposition Driver" +PROJECT_NAME = "Fixposition Driver LIB" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version @@ -771,7 +771,7 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = include src submodules README.md +INPUT = include src submodules README.md # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses diff --git a/fixposition_driver_lib/README.md b/fixposition_driver_lib/README.md new file mode 100644 index 0000000..077068b --- /dev/null +++ b/fixposition_driver_lib/README.md @@ -0,0 +1,37 @@ +# Fixposition Driver Lib + +This is a CMake library used to parse [Fixposition ASCII messages](../README.md#fixposition-ascii-messages). The message content will be converted into a generic struct and can be processed further from there. + +It can be built using plain CMake, or using catkin or colcon depending on which ROS version is used. + +## Dependencies +- [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page), tested with version [3.3.7](https://gitlab.com/libeigen/eigen/-/releases/3.3.7) +- [Boost](https://www.boost.org/), tested with version [1.65.0](https://www.boost.org/users/history/version_1_65_0.html) +- [CMake](https://cmake.org/) + +ROS1: +- [Catkin](http://wiki.ros.org/catkin) + +ROS2 +- [Colcon](https://colcon.readthedocs.io/en/released/) + + +## Build +### CMake +from the repository root dir: +``` +cd fixposition_driver_lib +mkdir build +cd build +cmake .. +make +``` + +### Catkin +clone this repository under the `src` folder in the catkin workspace, and then run `catkin build fixposition_driver_lib` or if you want to build all packages, run `catkin build`. + +### Colcon +clone this repository under the `src` folder in the colcon workspace, and then run `colcon build --packages-up-to fixposition_driver_lib` or if you want to build all packages, run `colcon build` + +### Use this library in other projects: +A cmake file is provided in the cmake directory, please see [fixposition_driver_ros1](../fixposition_driver_ros1) or [fixposition_driver_ros2](../fixposition_driver_ros2) as examples of using this library. diff --git a/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in b/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in new file mode 100644 index 0000000..cb4bbf3 --- /dev/null +++ b/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in @@ -0,0 +1,8 @@ +@PACKAGE_INIT@ + +set(@PROJECT_NAME@_FOUND ON) +set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include") +set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib") +set(@PROJECT_NAME@_LIBRARIES "@PACKAGE_LIBRARIES@") + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/base_converter.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/base_converter.hpp new file mode 100644 index 0000000..45917a7 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/base_converter.hpp @@ -0,0 +1,110 @@ +/** + * @file + * @brief Base Converter to define the interfaces + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__ + +/* SYSTEM / STL */ +#include +#include + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { +class BaseConverter { + public: + BaseConverter() = default; + ~BaseConverter() = default; + + /** + * @brief Virtual interface to convert the split tokens into ros messages + * + * @param[in] tokens vector of strings split by comma + */ + virtual void ConvertTokens(const std::vector& tokens) = 0; + + /** + * @brief Check the header and version of the corresponding message to make sure it is correct + * + * @param[in] msg_header expected header + * @param[in] msg_version expected version + * @return true correct + * @return false incorrect + */ + virtual bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) = 0; +}; + +//=================================================== + +/** + * @brief Helper function to convert string into double. If string is empty then 0.0 is returned + * + * @param[in] in_str + * @return double + */ +inline double StringToDouble(const std::string& in_str) { return in_str.empty() ? 0. : std::stod(in_str); } + +/** + * @brief Make sure the quaternion is unit quaternion + * + * @param[in] quat Eigen::Quaterniond to be checked + * @return true valid quaternion + * @return false invalid quaternion + */ +inline bool CheckQuat(const Eigen::Quaterniond& quat) { return abs(quat.squaredNorm() - 1.0) <= 1e-3; } + +/** + * @brief Convert 3 string values into a Eigen::Vector3d + * + * @param[in] x + * @param[in] y + * @param[in] z + * @return Eigen::Vector3d + */ +inline Eigen::Vector3d Vector3ToEigen(const std::string& x, const std::string& y, const std::string& z) { + return Eigen::Vector3d(StringToDouble(x), StringToDouble(y), StringToDouble(z)); +} + +/** + * @brief convert 4 string values into a Eigen::Quaterniond + * + * @param[in] w + * @param[in] x + * @param[in] y + * @param[in] z + * @return Eigen::Quaterniond + */ +inline Eigen::Quaterniond Vector4ToEigen(const std::string& w, const std::string& x, const std::string& y, + const std::string& z) { + return Eigen::Quaterniond(StringToDouble(w), StringToDouble(x), StringToDouble(y), StringToDouble(z)); +} + +/** + * @brief Helper function to convert strings containing gps_wno and gps_tow into ros::Time + * + * @param[in] gps_wno + * @param[in] gps_tow + * @return ros::Time + */ +inline times::GpsTime ConvertGpsTime(const std::string& gps_wno, const std::string& gps_tow) { + if (!gps_wno.empty() && !gps_tow.empty()) { + return times::GpsTime(std::stoi(gps_wno), std::stod(gps_tow)); + } else { + return times::GpsTime(0, 0); + } +} +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__ diff --git a/fixposition_driver/include/fixposition_driver/converter/imu.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/imu.hpp similarity index 52% rename from fixposition_driver/include/fixposition_driver/converter/imu.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/converter/imu.hpp index 1aebcad..2c0cc1b 100644 --- a/fixposition_driver/include/fixposition_driver/converter/imu.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/imu.hpp @@ -9,30 +9,28 @@ * /__/ \__\ * */ -#ifndef __FIXPOSITION_DRIVER_CONVERTER_IMU__ -#define __FIXPOSITION_DRIVER_CONVERTER_IMU__ +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_IMU__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_IMU__ -/* ROS */ -#include -#include +/* EXTERNAL */ +#include /* PACKAGE */ -#include -#include +#include +#include +#include namespace fixposition { class ImuConverter : public BaseConverter { public: + using ImuObserver = std::function; /** * @brief Construct a new ImuConverter * */ - ImuConverter(ros::NodeHandle& nh, const bool bias_correction) - : imu_pub_( - nh.advertise((bias_correction ? "/fixposition/corrimu" : "/fixposition/rawimu"), 100)), - header_(bias_correction ? "CORRIMU" : "RAWIMU"), - bias_correction_(bias_correction) {} + ImuConverter(const bool bias_correction) + : header_(bias_correction ? "CORRIMU" : "RAWIMU"), bias_correction_(bias_correction) {} ~ImuConverter() = default; @@ -41,21 +39,30 @@ class ImuConverter : public BaseConverter { } /** - * @brief Take comma-delimited tokens of FP,RAWIMU or FP,RAWIMU message and publish them as ros message - * Examples: + * @brief Take comma-delimited tokens of FP,RAWIMU or FP,RAWIMU message, convert to Data structs and if available, + * call observers + * Example: * $FP,RAWIMU,1,2197,126191.777855,-0.199914,0.472851,9.917973,0.023436,0.007723,0.002131*34 * $FP,CORRIMU,1,2197,126191.777855,-0.195224,0.393969,9.869998,0.013342,-0.004620,-0.000728*7D * * @param[in] tokens message split in tokens */ - void ConvertTokensAndPublish(const std::vector& tokens) final; + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(ImuObserver ob) { obs_.push_back(ob); } private: - ros::Publisher imu_pub_; + ImuData msg_; + std::vector obs_; const std::string header_; const bool bias_correction_; static constexpr const int kVersion_ = 1; }; } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_CONVERTER_IMU__ +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_IMU__ diff --git a/fixposition_driver/include/fixposition_driver/converter/llh.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/llh.hpp similarity index 52% rename from fixposition_driver/include/fixposition_driver/converter/llh.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/converter/llh.hpp index b373916..a70c532 100644 --- a/fixposition_driver/include/fixposition_driver/converter/llh.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/llh.hpp @@ -10,51 +10,57 @@ * */ -#ifndef __FIXPOSITION_DRIVER_CONVERTER_LLH__ -#define __FIXPOSITION_DRIVER_CONVERTER_LLH__ +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_LLH__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_LLH__ /* SYSTEM / STL */ /* EXTERNAL */ -/* ROS */ -#include - /* PACKAGE */ -#include - -#include -#include +#include +#include +#include namespace fixposition { class LlhConverter : public BaseConverter { public: + using LlhObserver = std::function; /** * @brief Construct a new LlhConverter * */ - LlhConverter(ros::NodeHandle& nh) - : navsatfix_pub_(nh.advertise("/fixposition/navsatfix", 100)) {} + LlhConverter() {} ~LlhConverter() = default; bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final { return msg_header == header_ && std::stoi(msg_version) == kVersion_; } + /** - * @brief Take comma-delimited tokens of FP,LLH message and publish them as ros message + * @brief Take comma-delimited tokens of FP,LLH message, convert to Data structs and if available, + * call observers * Example: * $FP,LLH,1,2197,126191.765,47.398826818,8.458494107,457.518,0.31537,1.0076,0.072696,-0.080012,0.0067274,-0.011602*4E\r\n * * @param[in] tokens message split in tokens */ - void ConvertTokensAndPublish(const std::vector& tokens) final; + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(LlhObserver ob) { obs_.push_back(ob); } private: - ros::Publisher navsatfix_pub_; + NavSatFixData msg_; + std::vector obs_; const std::string header_ = "LLH"; static constexpr const int kVersion_ = 1; }; } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_CONVERTER_LLH__ +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_LLH__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/msg_data.hpp new file mode 100644 index 0000000..5e931c4 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/msg_data.hpp @@ -0,0 +1,115 @@ +/** + * @file + * @brief Declaration of Data types + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_MSG_DATA__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_MSG_DATA__ + +/* EXTERNAL */ +#include +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +struct ImuData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + times::GpsTime stamp; + std::string frame_id; + Eigen::Vector3d linear_acceleration; + Eigen::Vector3d angular_velocity; + ImuData() { + linear_acceleration.setZero(); + angular_velocity.setZero(); + } +}; + +struct TfData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + times::GpsTime stamp; + std::string frame_id; + std::string child_frame_id; + Eigen::Vector3d translation; + Eigen::Quaterniond rotation; + TfData() : frame_id(""), child_frame_id("") { + translation.setZero(); + rotation.setIdentity(); + } +}; + +struct PoseWithCovData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d position; + Eigen::Quaterniond orientation; + Eigen::Matrix cov; + PoseWithCovData() { + position.setZero(); + orientation.setIdentity(); + cov.setZero(); + } +}; + +struct TwistWithCovData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d linear; + Eigen::Vector3d angular; + Eigen::Matrix cov; + TwistWithCovData() { + linear.setZero(); + angular.setZero(); + cov.setZero(); + } +}; + +struct OdometryData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + times::GpsTime stamp; + std::string frame_id; + std::string child_frame_id; + PoseWithCovData pose; + TwistWithCovData twist; +}; + +struct VrtkData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + times::GpsTime stamp; + std::string frame_id; + std::string pose_frame; + std::string kin_frame; + PoseWithCovData pose; + TwistWithCovData velocity; + Eigen::Vector3d acceleration; + int fusion_status; + int imu_bias_status; + int gnss_status; + int wheelspeed_status; + std::string version; + VrtkData() : fusion_status(-1), imu_bias_status(-1), gnss_status(-1), wheelspeed_status(-1), version("") { + acceleration.setZero(); + } +}; + +struct NavSatFixData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + times::GpsTime stamp; + std::string frame_id; + double latitude; + double longitude; + double altitude; + Eigen::Matrix cov; + int position_covariance_type; + NavSatFixData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); } +}; + +} // namespace fixposition +#endif //__FIXPOSITION_DRIVER_LIB_CONVERTER_MSG_DATA__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/odometry.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/odometry.hpp new file mode 100644 index 0000000..3485f7b --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/odometry.hpp @@ -0,0 +1,146 @@ +/** + * @file + * @brief Declaration of OdometryConverter + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMETRY__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMETRY__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ +#include +#include + +/* PACKAGE */ + +#include +#include +#include + +namespace fixposition { + +class OdometryConverter : public BaseConverter { + public: + /** + * @brief Data for Messages published from the ODOMETRY msg + * + */ + struct Msgs { + OdometryData odometry; + ImuData imu; + OdometryData odometry_enu0; + VrtkData vrtk; + Eigen::Vector3d eul; + TfData tf_ecef_poi; + TfData tf_ecef_enu; + TfData tf_ecef_enu0; + }; + + using OdometryObserver = std::function; + + /** + * @brief Construct a new Fixposition Msg Converter object + * + */ + OdometryConverter() : BaseConverter(), tf_ecef_enu0_set_(false) { + msgs_.tf_ecef_enu0.frame_id = "ECEF"; + msgs_.tf_ecef_enu0.child_frame_id = "FP_ENU0"; + } + + ~OdometryConverter() = default; + + bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final { + return msg_header == header_ && std::stoi(msg_version) == kVersion_; + } + + /** + * @brief Comma Delimited FP,ODOMETRY message, convert to Data structs and if available, + * call observers + * Example: + * $FP,ODOMETRY,1,2216,509791.426,4282251.9970,641470.7361,4668050.6007,-0.016059,0.359036,0.067523, + * 0.930740,9.6271,0.0718,-0.0231,0.01118,0.00952,0.03600,1.1313,0.3863,9.5607,4,1,8,1,0.01411,0.00720,0.01547, + * 0.00090,-0.00105,-0.00832,0.00021,0.00016,0.00019,-0.00000,0.00001,0.00008,0.07652, + * 0.05768,0.05234,0.00309,-0.00001,0.00173,fp_release_vr2_2.46.1_124*7D + * + * @param[in] state state message as string + * @return nav_msgs::Odometry message + */ + virtual void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(OdometryObserver ob) { obs_.push_back(ob); } + + private: + const std::string header_ = "ODOMETRY"; + static constexpr const int kVersion_ = 1; + + //! transform between ECEF and ENU0 + bool tf_ecef_enu0_set_; //!< flag to indicate if the tf is already set + Eigen::Vector3d t_ecef_enu0_; + Eigen::Quaterniond q_ecef_enu0_; + + Msgs msgs_; + std::vector obs_; +}; + +/** + * @brief Build a 6x6 covariance matrix which is 2 independent 3x3 matrices + * + * [xx, xy, xz, 0, 0, 0, + * xy, yy, yz, 0, 0, 0, + * xz, yz, zz, 0, 0, 0, + * 0, 0, 0, xx1, xy1, xz1, + * 0, 0, 0, xy1, yy1, yz1, + * 0, 0, 0, xz1, yz1, zz1] + * + * @param[in] xx + * @param[in] yy + * @param[in] zz + * @param[in] xy + * @param[in] yz + * @param[in] xz + * @param[in] xx1 + * @param[in] yy1 + * @param[in] zz1 + * @param[in] xy1 + * @param[in] yz1 + * @param[in] xz1 + * @return Eigen::Matrix the 6x6 matrix + */ +inline Eigen::Matrix BuildCovMat6D(const double xx, const double yy, const double zz, const double xy, + const double yz, const double xz, double xx1, const double yy1, + const double zz1, const double xy1, const double yz1, double xz1) { + Eigen::Matrix cov; + // Diagonals + cov(0, 0) = xx; // 0 + cov(1, 1) = yy; // 7 + cov(2, 2) = zz; // 14 + cov(3, 3) = xx1; // 21 + cov(4, 4) = yy1; // 28 + cov(5, 5) = zz1; // 35 + + // Rest of values + cov(1, 0) = cov(0, 1) = xy; // 1 = 6 + cov(2, 1) = cov(1, 2) = yz; // 8 = 13 + cov(2, 0) = cov(0, 2) = xz; // 2 = 12 + cov(4, 3) = cov(3, 4) = xy1; // 22 = 27 + cov(5, 4) = cov(4, 5) = yz1; // 29 = 34 + cov(5, 3) = cov(3, 5) = xz1; // 23 = 33 + + return cov; +} + +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMETRY__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/tf.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/tf.hpp new file mode 100644 index 0000000..4355f2c --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/tf.hpp @@ -0,0 +1,64 @@ +/** + * @file + * @brief Declaration of TfConverter + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_TF__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_TF__ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include +#include + +namespace fixposition { + +class TfConverter : public BaseConverter { + public: + using TfObserver = std::function; + /** + * @brief Construct a new Fixposition Msg Converter object + * + */ + TfConverter() = default; + + ~TfConverter() = default; + + bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final { + return msg_header == header_ && std::stoi(msg_version) == kVersion_; + } + /** + * @brief ake comma-delimited tokens of FP,TF message, convert to Data structs and if available, + * call observers + * + * @param[in] state state message as string + * @return nav_msgs::Odometry message + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(TfObserver ob) { obs_.push_back(ob); } + + private: + TfData msg_; + std::vector obs_; + + const std::string header_ = "TF"; + + static constexpr const int kVersion_ = 1; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_TF__ diff --git a/fixposition_driver/include/fixposition_driver/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp similarity index 67% rename from fixposition_driver/include/fixposition_driver/fixposition_driver.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp index 96547d8..e0e3214 100644 --- a/fixposition_driver/include/fixposition_driver/fixposition_driver.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp @@ -10,8 +10,8 @@ * */ -#ifndef __FIXPOSITION_DRIVER_FIXPOSITION_DRIVER__ -#define __FIXPOSITION_DRIVER_FIXPOSITION_DRIVER__ +#ifndef __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ +#define __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ /* SYSTEM / STL */ #include @@ -20,19 +20,9 @@ /* EXTERNAL */ -/* ROS */ -#include -#include -#include -#include - -/* PACKAGE */ -#include -#include - -#include -#include -#include +#include +#include +#include namespace fixposition { @@ -41,9 +31,8 @@ class FixpositionDriver { /** * @brief Construct a new FixpositionDriver object * - * @param[in] nh node handle */ - FixpositionDriver(ros::NodeHandle *nh); + FixpositionDriver(const FixpositionDriverParams& params); /** * @brief Destroy the Fixposition Driver object, close all open connections @@ -55,22 +44,22 @@ class FixpositionDriver { * @brief Run in Loop the Read Convert and Publish cycle * */ - void Run(); + bool RunOnce(); - private: + protected: /** * @brief * * @param[in] msg */ - void WsCallback(const fixposition_driver::SpeedConstPtr& msg); + void WsCallback(const std::vector& speeds); /** * @brief Convert the string using correct converter * * @param[in] msg NMEA like string to be converted. $HEADER,,,,,,,*CHECKSUM */ - void ConvertAndPublish(const std::string &msg); + void ConvertAndPublish(const std::string& msg); /** * @brief Initialize convertes based on config @@ -112,19 +101,16 @@ class FixpositionDriver { */ bool CreateSerialConnection(); - ros::NodeHandle nh_; - ros::Subscriber ws_sub_; //!< wheelspeed message subscriber + FixpositionDriverParams params_; RAWDMI rawdmi_; //!< RAWDMI msg struct - FixpositionDriverParams params_; - std::unordered_map> converters_; //!< converters corresponding to the input formats - int client_fd_ = -1; //!< TCP or Serial file descriptor - int connection_status_ = -1; //!< + int client_fd_ = -1; //!< TCP or Serial file descriptor + int connection_status_ = -1; struct termios options_save_; }; } // namespace fixposition -#endif //__FIXPOSITION_DRIVER_FIXPOSITION_DRIVER__ +#endif //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ diff --git a/fixposition_driver/include/fixposition_driver/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp similarity index 77% rename from fixposition_driver/include/fixposition_driver/helper.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp index 379807d..7d10f04 100644 --- a/fixposition_driver/include/fixposition_driver/helper.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp @@ -10,8 +10,8 @@ * */ -#ifndef __FIXPOSITION_DRIVER_HELPER__ -#define __FIXPOSITION_DRIVER_HELPER__ +#ifndef __FIXPOSITION_DRIVER_LIB_HELPER__ +#define __FIXPOSITION_DRIVER_LIB_HELPER__ /* SYSTEM / STL */ #include @@ -41,12 +41,5 @@ void SplitMessage(std::vector& tokens, const std::string& msg, cons */ int IsNmeaMessage(const char* buf, const int size); -/** - * @brief Send ROS Fatal error and exit - * - * @param[in] error Error msg to be sent - */ -void ROSFatalError(const std::string& error); - } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_HELPER__ +#endif // __FIXPOSITION_DRIVER_LIB_HELPER__ diff --git a/fixposition_driver/include/fixposition_driver/params.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp similarity index 62% rename from fixposition_driver/include/fixposition_driver/params.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/params.hpp index e535836..3d003ad 100644 --- a/fixposition_driver/include/fixposition_driver/params.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp @@ -10,8 +10,8 @@ * */ -#ifndef __FIXPOSITION_DRIVER_PARAMS_HPP__ -#define __FIXPOSITION_DRIVER_PARAMS_HPP__ +#ifndef __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__ +#define __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__ /* SYSTEM / STL */ #include @@ -34,32 +34,14 @@ struct FpOutputParams { std::string ip; //!< IP address for TCP connection std::string port; //!< Port for TCP connection int baudrate; //!< baudrate of serial connection - - /** - * @brief Load all parameters from ROS parameter server - * - * @param[in] ns namespace to load the parameters from - * @return true success - * @return false fail - */ - bool LoadFromRos(const std::string &ns); }; struct CustomerInputParams { std::string speed_topic; - /** - * @brief Load all parameters from ROS parameter server - * - * @param[in] ns namespace to load the parameters from - * @return true success - * @return false fail - */ - bool LoadFromRos(const std::string &ns); }; struct FixpositionDriverParams { FpOutputParams fp_output; CustomerInputParams customer_input; - bool LoadFromRos(const std::string &ns = "~/"); }; } // namespace fixposition diff --git a/fixposition_driver/include/fixposition_driver/rawdmi.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/rawdmi.hpp similarity index 76% rename from fixposition_driver/include/fixposition_driver/rawdmi.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/rawdmi.hpp index 55e8696..5d29bcf 100644 --- a/fixposition_driver/include/fixposition_driver/rawdmi.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/rawdmi.hpp @@ -1,17 +1,17 @@ /** * @file * @brief Declaration of RAWDMI message struct - * + * * ___ ___ * \ \ / / * \ \/ / Fixposition AG * / /\ \ All right reserved. * /__/ \__\ - * + * */ -#ifndef __FIXPOSITION_DRIVER_RAWDMI__ -#define __FIXPOSITION_DRIVER_RAWDMI__ +#ifndef __FIXPOSITION_DRIVER_LIB_RAWDMI__ +#define __FIXPOSITION_DRIVER_LIB_RAWDMI__ #include @@ -19,12 +19,12 @@ namespace fixposition { /** * @brief CRC32 calculation - * - * @param[in] data - * @param[in] size - * @return uint32_t + * + * @param[in] data + * @param[in] size + * @return uint32_t */ -inline uint32_t crc32(const uint8_t *data, const int size) { +inline uint32_t crc32(const uint8_t* data, const int size) { uint32_t crc = 0; for (int i = 0; i < size; i++) { crc ^= data[i]; @@ -41,7 +41,7 @@ inline uint32_t crc32(const uint8_t *data, const int size) { /** * @brief RAWDMI message struct - * + * */ struct RAWDMI { uint8_t head1; @@ -60,4 +60,4 @@ struct RAWDMI { } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_RAWDMI__ +#endif // __FIXPOSITION_DRIVER_LIB_RAWDMI__ diff --git a/fixposition_driver/include/fixposition_driver/time_conversions.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp similarity index 82% rename from fixposition_driver/include/fixposition_driver/time_conversions.hpp rename to fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp index 0362da8..c66fe61 100644 --- a/fixposition_driver/include/fixposition_driver/time_conversions.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp @@ -10,12 +10,8 @@ * */ -#ifndef __FIXPOSITION_DRIVER_TIME_CONVERSIONS__ -#define __FIXPOSITION_DRIVER_TIME_CONVERSIONS__ - -#include -#include -#include +#ifndef __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__ +#define __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__ #include @@ -29,6 +25,7 @@ namespace times { * */ using TIME_NS_T = uint64_t; + namespace Constants { static constexpr int gps_leap_time_s = 18; /// gps leap seconds @@ -42,6 +39,7 @@ static constexpr int sec_per_day = 86400; //!< seconds per day static constexpr double ns_to_sec = 1.0e-9; //!< convert ns to seconds static constexpr double sec_to_ns = 1.0e9; //!< convert seconds to ns + } // namespace Constants /** @@ -61,7 +59,7 @@ class GpsTime { * @brief Construct a new GpsTime object * */ - GpsTime() {} + GpsTime() : wno(0), tow(0.0) {} /** * @brief Construct a new GpsTime object @@ -75,15 +73,15 @@ class GpsTime { tow = tow - delta_week * Constants::sec_per_week; } - GpsTime(GpsTime const &) = default; + GpsTime(GpsTime const&) = default; - GpsTime &operator=(const GpsTime gws) { + GpsTime& operator=(const GpsTime gws) { tow = gws.tow; wno = gws.wno; return *this; } - GpsTime &operator+=(const double sec) { + GpsTime& operator+=(const double sec) { double gps_sec_tmp = tow + sec; // std::cout << gps_sec_tmp << std::endl; int delta_week = std::floor(gps_sec_tmp / Constants::sec_per_week); @@ -93,18 +91,18 @@ class GpsTime { return *this; } - GpsTime &operator+=(const GpsTime in) { + GpsTime& operator+=(const GpsTime in) { double d_sec = in.wno * Constants::sec_per_week + in.tow; *this += d_sec; return *this; } - GpsTime &operator-=(const double sec) { + GpsTime& operator-=(const double sec) { *this += -sec; return *this; } - GpsTime &operator-=(const GpsTime in) { + GpsTime& operator-=(const GpsTime in) { double d_sec = in.wno * Constants::sec_per_week + in.tow; *this -= d_sec; return *this; @@ -163,7 +161,7 @@ class GpsTime { * @param[in] gps_time * @return std::ostream& */ - friend std::ostream &operator<<(std::ostream &stream, const GpsTime &gps_time) { + friend std::ostream& operator<<(std::ostream& stream, const GpsTime& gps_time) { std::string week = std::to_string(gps_time.wno); week.resize(4, ' '); stream << week << " "; @@ -188,7 +186,7 @@ class GpsTime { * @param[in] gps_time * @return BOOST_POSIX::ptime */ -inline BOOST_POSIX::ptime GpsTimeToPtime(const GpsTime &gps_time) { +inline BOOST_POSIX::ptime GpsTimeToPtime(const GpsTime& gps_time) { TIME_NS_T micro_s = (gps_time.wno * Constants::sec_per_week + gps_time.tow - Constants::gps_leap_time_s) * 1e6; return Constants::gps_epoch_begin + BOOST_POSIX::microseconds(micro_s); } @@ -200,7 +198,7 @@ inline BOOST_POSIX::ptime GpsTimeToPtime(const GpsTime &gps_time) { * @param[in] boost_ptime * @return GpsTime */ -inline GpsTime PtimeToGpsTime(const BOOST_POSIX::ptime &boost_ptime) { +inline GpsTime PtimeToGpsTime(const BOOST_POSIX::ptime& boost_ptime) { BOOST_POSIX::time_duration gps_duration = boost_ptime - Constants::gps_epoch_begin; int weekcount = gps_duration.total_seconds() / Constants::sec_per_week; double sec_in_week = @@ -208,21 +206,6 @@ inline GpsTime PtimeToGpsTime(const BOOST_POSIX::ptime &boost_ptime) { return GpsTime(weekcount, sec_in_week); } -/** - * @brief Convert GpsTime to ROS Time - * - * @param[in] input - * @return ros::Time - */ -inline ros::Time GpsTimeToRosTime(GpsTime input) { return ros::Time::fromBoost(GpsTimeToPtime(input)); } - -/** - * @brief Convert ROS Time to GpsTime - * - * @param[in] ros_time - * @return GpsTime - */ -inline GpsTime RosTimeToGpsTime(const ros::Time &ros_time) { return PtimeToGpsTime(ros_time.toBoost()); } } // namespace times } // namespace fixposition -#endif // __FIXPOSITION_DRIVER_TIME_CONVERSIONS__ +#endif // __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__ diff --git a/fixposition_driver_lib/package.xml b/fixposition_driver_lib/package.xml new file mode 100644 index 0000000..42a797c --- /dev/null +++ b/fixposition_driver_lib/package.xml @@ -0,0 +1,26 @@ + + + fixposition_driver_lib + 5.0.0 + Fixposition ROS Driver Lib + + Fixposition AG + + MIT + + catkin + ament_cmake + ament_cmake_gtest + roscpp + tf + nav_msgs + sensor_msgs + std_msgs + geometry_msgs + message_generation + message_runtime + fixposition_gnss_tf + + cmake + + diff --git a/fixposition_driver/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp similarity index 62% rename from fixposition_driver/src/fixposition_driver.cpp rename to fixposition_driver_lib/src/fixposition_driver.cpp index d253f12..9307e4b 100644 --- a/fixposition_driver/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -16,26 +16,17 @@ #include /* PACKAGE */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace fixposition { -FixpositionDriver::FixpositionDriver(ros::NodeHandle *nh) : nh_(*nh) { - // read parameters - if (!params_.LoadFromRos()) { - ROSFatalError("Parameter Loading failed, shutting down..."); - } - +FixpositionDriver::FixpositionDriver(const FixpositionDriverParams& params) : params_(params) { Connect(); - ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 100, - &FixpositionDriver::WsCallback, this, - ros::TransportHints().tcpNoDelay()); - // static headers rawdmi_.head1 = 0xaa; rawdmi_.head2 = 0x44; @@ -53,7 +44,7 @@ FixpositionDriver::FixpositionDriver(ros::NodeHandle *nh) : nh_(*nh) { // initialize converters if (!InitializeConverters()) { - ROSFatalError("Could not initialize output converter!"); + std::cerr << "Could not initialize output converter!\n"; } } @@ -75,29 +66,31 @@ bool FixpositionDriver::Connect() { return CreateSerialConnection(); break; default: - ROSFatalError("Unknown connection type."); + std::cerr << "Unknown connection type!\n"; return false; } } -void FixpositionDriver::WsCallback(const fixposition_driver::SpeedConstPtr& msg) { - if (msg->speeds.size() == 1) { - rawdmi_.dmi1 = msg->speeds[0]; +void FixpositionDriver::WsCallback(const std::vector& speeds) { + if (speeds.size() == 1) { + rawdmi_.dmi1 = speeds[0]; rawdmi_.mask = (1 << 0) | (0 << 1) | (0 << 2) | (0 << 3); - } else if (msg->speeds.size() == 4) { - rawdmi_.dmi1 = msg->speeds[0]; - rawdmi_.dmi2 = msg->speeds[1]; - rawdmi_.dmi3 = msg->speeds[2]; - rawdmi_.dmi4 = msg->speeds[3]; + } else if (speeds.size() == 2) { + rawdmi_.dmi1 = speeds[0]; + rawdmi_.dmi2 = speeds[1]; + rawdmi_.mask = (1 << 0) | (1 << 1) | (0 << 2) | (0 << 3) | (1 << 11); + } else if (speeds.size() == 4) { + rawdmi_.dmi1 = speeds[0]; + rawdmi_.dmi2 = speeds[1]; + rawdmi_.dmi3 = speeds[2]; + rawdmi_.dmi4 = speeds[3]; rawdmi_.mask = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3); } else { - ROS_WARN_THROTTLE(1, "Invalid speed message with size %lu, the size should be either 1 or 4!", - msg->speeds.size()); return; } // Calculate CRC - const uint32_t checksum = crc32((const uint8_t *)&rawdmi_, sizeof(rawdmi_)); + const uint32_t checksum = crc32((const uint8_t*)&rawdmi_, sizeof(rawdmi_)); // Compose entire message uint8_t message[sizeof(rawdmi_) + sizeof(checksum)]; @@ -108,42 +101,33 @@ void FixpositionDriver::WsCallback(const fixposition_driver::SpeedConstPtr& msg) } bool FixpositionDriver::InitializeConverters() { - for (const auto format : params_.fp_output.formats) { + for (const auto& format : params_.fp_output.formats) { if (format == "ODOMETRY") { - converters_["ODOMETRY"] = std::unique_ptr(new OdometryConverter(nh_)); - converters_["TF"] = std::unique_ptr(new TfConverter(nh_)); + converters_["ODOMETRY"] = std::unique_ptr(new OdometryConverter()); + converters_["TF"] = std::unique_ptr(new TfConverter()); } else if (format == "LLH") { - converters_["LLH"] = std::unique_ptr(new LlhConverter(nh_)); + converters_["LLH"] = std::unique_ptr(new LlhConverter()); } else if (format == "RAWIMU") { - converters_["RAWIMU"] = std::unique_ptr(new ImuConverter(nh_, false)); + converters_["RAWIMU"] = std::unique_ptr(new ImuConverter(false)); } else if (format == "CORRIMU") { - converters_["CORRIMU"] = std::unique_ptr(new ImuConverter(nh_, true)); + converters_["CORRIMU"] = std::unique_ptr(new ImuConverter(true)); } else if (format == "TF") { - if (converters_.find("TF") != converters_.end()) { - converters_["TF"] = std::unique_ptr(new TfConverter(nh_)); + if (converters_.find("TF") == converters_.end()) { + converters_["TF"] = std::unique_ptr(new TfConverter()); } } else { - ROS_INFO_STREAM("Unknown input format: " << format); + std::cerr << "Unknown input format: " << format << "\n"; } } return !converters_.empty(); } -void FixpositionDriver::Run() { - ros::Rate rate(params_.fp_output.rate); - while (ros::ok()) { - if ((client_fd_ > 0) && (connection_status_ == 0) && ReadAndPublish()) { - ros::spinOnce(); - rate.sleep(); - } else { - ROS_INFO("Reconnecting in %.1f seconds ...", params_.fp_output.reconnect_delay); - close(client_fd_); - client_fd_ = -1; - - ros::spinOnce(); - ros::Duration(params_.fp_output.reconnect_delay).sleep(); - - Connect(); - } +bool FixpositionDriver::RunOnce() { + if ((client_fd_ > 0) && (connection_status_ == 0) && ReadAndPublish()) { + return true; + } else { + close(client_fd_); + client_fd_ = -1; + return false; } } @@ -152,15 +136,15 @@ bool FixpositionDriver::ReadAndPublish() { ssize_t rv; if (params_.fp_output.type == INPUT_TYPE::TCP) { - rv = recv(client_fd_, (void *)&readBuf, sizeof(readBuf), MSG_DONTWAIT); + rv = recv(client_fd_, (void*)&readBuf, sizeof(readBuf), MSG_DONTWAIT); } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) { - rv = read(client_fd_, (void *)&readBuf, sizeof(readBuf)); + rv = read(client_fd_, (void*)&readBuf, sizeof(readBuf)); } else { rv = 0; } if (rv == 0) { - ROS_ERROR_STREAM("Connection closed."); + std::cerr << "Connection closed.\n"; return false; } @@ -169,12 +153,12 @@ bool FixpositionDriver::ReadAndPublish() { return true; } if (rv < 0) { - ROS_ERROR_STREAM("Connection error."); + std::cerr << "Connection error.\n"; return false; } // find start of a NMEA style message - char *start = (char *)memchr(readBuf, '$', rv); + char* start = (char*)memchr(readBuf, '$', rv); while (start != NULL) { // check if it is NMEA with correct checksum auto nmea_size = IsNmeaMessage(start, rv - (start - readBuf)); @@ -185,7 +169,7 @@ bool FixpositionDriver::ReadAndPublish() { ConvertAndPublish(msg); // move to next $ for next message - start = (char *)memchr(start + 1, '$', rv); + start = (char*)memchr(start + 1, '$', rv); } else { break; } @@ -194,7 +178,7 @@ bool FixpositionDriver::ReadAndPublish() { return true; } -void FixpositionDriver::ConvertAndPublish(const std::string &msg) { +void FixpositionDriver::ConvertAndPublish(const std::string& msg) { // split the msg into tokens, removing the *XX checksum std::vector tokens; std::size_t star_pos = msg.find_last_of("*"); @@ -208,9 +192,9 @@ void FixpositionDriver::ConvertAndPublish(const std::string &msg) { // Get the header of the sentence const std::string header = tokens.at(1); - // If we have a converter available, convert to ros. Currently supported are "FP" and "LLH" + // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU" if (converters_[header] != nullptr) { - converters_[header]->ConvertTokensAndPublish(tokens); + converters_[header]->ConvertTokens(tokens); } } @@ -219,10 +203,10 @@ bool FixpositionDriver::CreateTCPSocket() { client_fd_ = socket(AF_INET, SOCK_STREAM, 0); if (client_fd_ < 0) { - ROS_ERROR_STREAM_THROTTLE(5, "Error in client creation."); + std::cerr << "Error in client creation.\n"; return false; } else { - ROS_INFO_STREAM("Client created."); + std::cout << "Client created.\n"; } server_address.sin_family = AF_INET; @@ -230,10 +214,10 @@ bool FixpositionDriver::CreateTCPSocket() { server_address.sin_port = htons(std::stoi(params_.fp_output.port)); server_address.sin_addr.s_addr = inet_addr(params_.fp_output.ip.c_str()); - connection_status_ = connect(client_fd_, (struct sockaddr *)&server_address, sizeof server_address); + connection_status_ = connect(client_fd_, (struct sockaddr*)&server_address, sizeof server_address); if (connection_status_ != 0) { - ROS_ERROR_STREAM("Error on connection of TCP socket: " << strerror(errno)); + std::cerr << "Error on connection of TCP socket: " << strerror(errno) << "\n"; return false; } return true; @@ -284,16 +268,16 @@ bool FixpositionDriver::CreateSerialConnection() { default: speed = B115200; - ROS_ERROR_STREAM("Unsupported baudrate: " << params_.fp_output.baudrate - << "\n\tsupported examples:\n\t9600, " - "19200, " - "38400, " - "57600\t\n115200\n230400\n460800\n500000\n921600\n1000000"); + std::cerr << "Unsupported baudrate: " << params_.fp_output.baudrate + << "\n\tsupported examples:\n\t9600, " + "19200, " + "38400, " + "57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n"; } if (client_fd_ == -1) { // Could not open the port. - ROS_ERROR_STREAM_THROTTLE(5, "Failed to open serial port " << strerror(errno)); + std::cerr << "Failed to open serial port " << strerror(errno) << "\n"; return false; } else { // Get current serial port options: diff --git a/fixposition_driver/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp similarity index 91% rename from fixposition_driver/src/helper.cpp rename to fixposition_driver_lib/src/helper.cpp index 973ce85..bd81874 100644 --- a/fixposition_driver/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -10,23 +10,14 @@ * */ -/* ROS */ -#include -#include - /* PACKAGE */ -#include +#include namespace fixposition { static constexpr const char kNmeaPreamble = '$'; static constexpr const int kLibParserMaxNmeaSize = 400; -void ROSFatalError(const std::string& error) { - ROS_ERROR_STREAM(error); - ros::shutdown(); -} - void SplitMessage(std::vector& tokens, const std::string& msg, const std::string& delim) { boost::split(tokens, msg, boost::is_any_of(delim)); } diff --git a/fixposition_driver_lib/src/imu.cpp b/fixposition_driver_lib/src/imu.cpp new file mode 100644 index 0000000..563d4a4 --- /dev/null +++ b/fixposition_driver_lib/src/imu.cpp @@ -0,0 +1,50 @@ +/** + * @file + * @brief Implementation of ImuConverter converter + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int msg_type_idx = 1; +static constexpr const int msg_version_idx = 2; +static constexpr const int gps_week_idx = 3; +static constexpr const int gps_tow_idx = 4; +static constexpr const int acc_x_idx = 5; +static constexpr const int acc_y_idx = 6; +static constexpr const int acc_z_idx = 7; +static constexpr const int rot_x_idx = 8; +static constexpr const int rot_y_idx = 9; +static constexpr const int rot_z_idx = 10; + +void ImuConverter::ConvertTokens(const std::vector& tokens) { + if (tokens.size() != 11) { + std::cout << "Error in parsing IMU string with" << tokens.size() << "fields! IMU message will be empty.\n"; + msg_ = ImuData(); + return; + } + // header stamps + msg_.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); + msg_.linear_acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx)); + msg_.angular_velocity = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx)); + + // process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_lib/src/llh.cpp b/fixposition_driver_lib/src/llh.cpp new file mode 100644 index 0000000..a060143 --- /dev/null +++ b/fixposition_driver_lib/src/llh.cpp @@ -0,0 +1,65 @@ +/** + * @file + * @brief Implementation of LlhConverter + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int msg_type_idx = 1; +static constexpr const int msg_versio_idx = 2; +static constexpr const int gps_week_idx = 3; +static constexpr const int gps_tow_idx = 4; +static constexpr const int latitude_idx = 5; +static constexpr const int longitude_idx = 6; +static constexpr const int height_idx = 7; +static constexpr const int pos_cov_ee_idx = 8; +static constexpr const int pos_cov_nn_idx = 9; +static constexpr const int pos_cov_uu_idx = 10; +static constexpr const int pos_cov_en_idx = 11; +static constexpr const int pos_cov_nu_idx = 12; +static constexpr const int pos_cov_eu_idx = 13; + +void LlhConverter::ConvertTokens(const std::vector& tokens) { + if (tokens.size() != 14) { + std::cout << "Error in parsing LLH string with" << tokens.size() + << "fields! NavSatFix message will be empty.\n"; + msg_ = NavSatFixData(); + return; + } + // header stamps + msg_.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); + + msg_.frame_id = "POI"; + msg_.latitude = StringToDouble(tokens.at(latitude_idx)); + msg_.longitude = StringToDouble(tokens.at(longitude_idx)); + msg_.altitude = StringToDouble(tokens.at(height_idx)); + + // Covariance diagonals + msg_.cov(0, 0) = StringToDouble(tokens.at(pos_cov_ee_idx)); + msg_.cov(1, 1) = StringToDouble(tokens.at(pos_cov_nn_idx)); + msg_.cov(2, 2) = StringToDouble(tokens.at(pos_cov_uu_idx)); + + // Rest of covariance fields + msg_.cov(0, 1) = msg_.cov(1, 0) = StringToDouble(tokens.at(pos_cov_en_idx)); + msg_.cov(0, 2) = msg_.cov(2, 0) = StringToDouble(tokens.at(pos_cov_nu_idx)); + msg_.cov(1, 2) = msg_.cov(2, 1) = StringToDouble(tokens.at(pos_cov_eu_idx)); + msg_.position_covariance_type = 3; + + // process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_lib/src/odometry.cpp b/fixposition_driver_lib/src/odometry.cpp new file mode 100644 index 0000000..3a90c79 --- /dev/null +++ b/fixposition_driver_lib/src/odometry.cpp @@ -0,0 +1,223 @@ +/** + * @file + * @brief Implementation of OdometryConverter + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +/* EXTERNAL */ +#include +#include + +/* FIXPOSITION */ +#include + +/* PACKAGE */ +#include +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int msg_type_idx = 1; +static constexpr const int msg_version_idx = 2; +static constexpr const int gps_week_idx = 3; +static constexpr const int gps_tow_idx = 4; +static constexpr const int pos_x_idx = 5; +static constexpr const int pos_y_idx = 6; +static constexpr const int pos_z_idx = 7; +static constexpr const int orientation_w_idx = 8; +static constexpr const int orientation_x_idx = 9; +static constexpr const int orientation_y_idx = 10; +static constexpr const int orientation_z_idx = 11; +static constexpr const int vel_x_idx = 12; +static constexpr const int vel_y_idx = 13; +static constexpr const int vel_z_idx = 14; +static constexpr const int rot_x_idx = 15; +static constexpr const int rot_y_idx = 16; +static constexpr const int rot_z_idx = 17; +static constexpr const int acc_x_idx = 18; +static constexpr const int acc_y_idx = 19; +static constexpr const int acc_z_idx = 20; +static constexpr const int fusion_status_idx = 21; +static constexpr const int imu_bias_status_idx = 22; +static constexpr const int gnss_fix_type_idx = 23; +static constexpr const int wheelspeed_status_idx = 24; +static constexpr const int pos_cov_xx_idx = 25; +static constexpr const int pos_cov_yy_idx = 26; +static constexpr const int pos_cov_zz_idx = 27; +static constexpr const int pos_cov_xy_idx = 28; +static constexpr const int pos_cov_yz_idx = 29; +static constexpr const int pos_cov_xz_idx = 30; +static constexpr const int orientation_cov_xx_idx = 31; +static constexpr const int orientation_cov_yy_idx = 32; +static constexpr const int orientation_cov_zz_idx = 33; +static constexpr const int orientation_cov_xy_idx = 34; +static constexpr const int orientation_cov_yz_idx = 35; +static constexpr const int orientation_cov_xz_idx = 36; +static constexpr const int vel_cov_xx_idx = 37; +static constexpr const int vel_cov_yy_idx = 38; +static constexpr const int vel_cov_zz_idx = 39; +static constexpr const int vel_cov_xy_idx = 40; +static constexpr const int vel_cov_yz_idx = 41; +static constexpr const int vel_cov_xz_idx = 42; +static constexpr const int sw_version_idx = 43; + +/** + * @brief Parse status flag field + * + * @param[in] tokens list of tokens + * @param[in] idx status flag index + * @return int + */ +int ParseStatusFlag(const std::vector& tokens, const int idx) { + if (tokens.at(idx).empty()) { + return -1; + } else { + return std::stoi(tokens.at(idx)); + } +} + +void OdometryConverter::ConvertTokens(const std::vector& tokens) { + if (tokens.size() != 44) { + std::cout << "Error in parsing Odometry string with " << tokens.size() + << " fields! odometry and status messages will be empty.\n"; + + msgs_ = Msgs(); + return; + } + const int fusion_status = ParseStatusFlag(tokens, fusion_status_idx); + + const bool fusion_init = fusion_status >= 3; + + // common data + const auto stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx)); + const Eigen::Vector3d t_ecef_body = + Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx)); + const Eigen::Quaterniond q_ecef_body = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx), + tokens.at(orientation_y_idx), tokens.at(orientation_z_idx)); + if (fusion_init) { + // TFs + msgs_.tf_ecef_enu.stamp = stamp; + msgs_.tf_ecef_poi.stamp = stamp; + msgs_.tf_ecef_enu.frame_id = "ECEF"; + msgs_.tf_ecef_poi.frame_id = "ECEF"; + msgs_.tf_ecef_poi.child_frame_id = "FP_POI"; + msgs_.tf_ecef_enu.child_frame_id = "FP_ENU"; // The ENU frame at the position of FP_POI + + // static TF ECEF ENU0 + if (!tf_ecef_enu0_set_ && msgs_.tf_ecef_enu0.translation.isZero()) { + // ENU0 frame is not yet set, set the same ENU tf to ENU0 + t_ecef_enu0_ = t_ecef_body; + q_ecef_enu0_ = Eigen::Quaterniond(gnss_tf::RotEnuEcef(t_ecef_body).transpose()); + msgs_.tf_ecef_enu0.translation = t_ecef_enu0_; + msgs_.tf_ecef_enu0.rotation = q_ecef_enu0_; + tf_ecef_enu0_set_ = true; + } + + // TF ECEF POI is basically the same as the odometry, containing the Pose of the POI in the ECEF Frame + msgs_.tf_ecef_poi.translation = (t_ecef_body); + msgs_.tf_ecef_poi.rotation = (q_ecef_body); + + // Quaternion from ECEF to Local ENU at POI + const Eigen::Quaterniond q_ecef_enu = Eigen::Quaterniond(gnss_tf::RotEnuEcef(t_ecef_body).transpose()); + // TF ECEF ENU + msgs_.tf_ecef_enu.translation = (t_ecef_body); + msgs_.tf_ecef_enu.rotation = (q_ecef_enu); + + // Send TFs + if (CheckQuat(msgs_.tf_ecef_poi.rotation)) { + } + if (CheckQuat(msgs_.tf_ecef_enu.rotation)) { + } + // Send Static TF ECEF ENU0 + if (tf_ecef_enu0_set_ && CheckQuat(msgs_.tf_ecef_enu0.rotation)) { + } + + msgs_.odometry.stamp = stamp; + msgs_.odometry.frame_id = "ECEF"; + msgs_.odometry.child_frame_id = "FP_POI"; + + msgs_.vrtk.stamp = stamp; + msgs_.vrtk.frame_id = "ECEF"; + msgs_.vrtk.pose_frame = "FP_POI"; + msgs_.vrtk.kin_frame = "FP_POI"; + + // Pose & Cov + msgs_.odometry.pose.position = (t_ecef_body); + msgs_.odometry.pose.orientation = (q_ecef_body); + msgs_.odometry.pose.cov = BuildCovMat6D( + StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)), + StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)), + StringToDouble(tokens.at(pos_cov_yz_idx)), StringToDouble(tokens.at(pos_cov_xz_idx)), + StringToDouble(tokens.at(orientation_cov_xx_idx)), StringToDouble(tokens.at(orientation_cov_yy_idx)), + StringToDouble(tokens.at(orientation_cov_zz_idx)), StringToDouble(tokens.at(orientation_cov_xy_idx)), + StringToDouble(tokens.at(orientation_cov_yz_idx)), StringToDouble(tokens.at(orientation_cov_xz_idx))); + msgs_.vrtk.pose = msgs_.odometry.pose; + + // Twist & Cov + // Linear + msgs_.odometry.twist.linear = Vector3ToEigen(tokens.at(vel_x_idx), tokens.at(vel_y_idx), tokens.at(vel_z_idx)); + // Angular + msgs_.odometry.twist.angular = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx)); + msgs_.odometry.twist.cov = BuildCovMat6D( + StringToDouble(tokens.at(vel_cov_xx_idx)), StringToDouble(tokens.at(vel_cov_yy_idx)), + StringToDouble(tokens.at(vel_cov_zz_idx)), StringToDouble(tokens.at(vel_cov_xy_idx)), + StringToDouble(tokens.at(vel_cov_yz_idx)), StringToDouble(tokens.at(vel_cov_xz_idx)), 0, 0, 0, 0, 0, 0); + msgs_.vrtk.velocity = msgs_.odometry.twist; + + // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll + msgs_.eul = gnss_tf::EcefPoseToEnuEul(t_ecef_body, q_ecef_body.toRotationMatrix()); + + // Odmetry msg ENU0 - FP_POI + msgs_.odometry_enu0.stamp = stamp; + msgs_.odometry_enu0.frame_id = "ENU0"; + msgs_.odometry_enu0.child_frame_id = "FP_POI"; + // Pose + // convert position in ECEF into position in ENU0 + const Eigen::Vector3d t_enu0_body = gnss_tf::TfEnuEcef(t_ecef_body, gnss_tf::TfWgs84LlhEcef(t_ecef_enu0_)); + const Eigen::Quaterniond q_enu0_body = q_ecef_enu0_.inverse() * q_ecef_body; + msgs_.odometry_enu0.pose.position = (t_enu0_body); + msgs_.odometry_enu0.pose.orientation = (q_enu0_body); + // Cov + Eigen::Matrix cov_ecef(msgs_.odometry.pose.cov); + const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0_.toRotationMatrix(); + Eigen::Map> cov_enu0(msgs_.odometry_enu0.pose.cov.data()); + msgs_.odometry_enu0.pose.cov.topLeftCorner(3, 3) = + rot_ecef_enu0 * cov_ecef.topLeftCorner(3, 3) * rot_ecef_enu0.transpose(); + msgs_.odometry_enu0.pose.cov.bottomRightCorner(3, 3) = + rot_ecef_enu0 * cov_ecef.bottomRightCorner(3, 3) * rot_ecef_enu0.transpose(); + + // Twist is the same as it is in the FP_POI frame + msgs_.odometry_enu0.twist = msgs_.odometry.twist; + } + + // Msgs + //!< Odmetry msg ECEF - FP_POI + + // Status, regardless of fusion_init + msgs_.vrtk.fusion_status = fusion_status; + msgs_.vrtk.imu_bias_status = ParseStatusFlag(tokens, imu_bias_status_idx); + msgs_.vrtk.gnss_status = ParseStatusFlag(tokens, gnss_fix_type_idx); + msgs_.vrtk.wheelspeed_status = ParseStatusFlag(tokens, wheelspeed_status_idx); + msgs_.vrtk.version = tokens.at(sw_version_idx).empty() ? "UNKNOWN" : tokens.at(sw_version_idx); + + // POI IMU Message + msgs_.imu.stamp = stamp; + msgs_.imu.frame_id = "FP_POI"; + // Omega + msgs_.imu.angular_velocity = msgs_.odometry.twist.angular; + // Acceleration + msgs_.imu.linear_acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx)); + + // process all observers + for (auto& ob : obs_) { + ob(msgs_); + } +} +} // namespace fixposition diff --git a/fixposition_driver_lib/src/tf.cpp b/fixposition_driver_lib/src/tf.cpp new file mode 100644 index 0000000..ad98825 --- /dev/null +++ b/fixposition_driver_lib/src/tf.cpp @@ -0,0 +1,55 @@ +/** + * @file + * @brief Implementation of TfConverter + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int msg_type_idx = 1; +static constexpr const int msg_version_idx = 2; +static constexpr const int from_frame_idx = 3; +static constexpr const int to_frame_idx = 4; +static constexpr const int translation_x_idx = 5; +static constexpr const int translation_y_idx = 6; +static constexpr const int translation_z_idx = 7; +static constexpr const int orientation_w_idx = 8; +static constexpr const int orientation_x_idx = 9; +static constexpr const int orientation_y_idx = 10; +static constexpr const int orientation_z_idx = 11; + +void TfConverter::ConvertTokens(const std::vector& tokens) { + if (tokens.size() != 12) { + std::cout << "Error in parsing TF string with " << tokens.size() << " fields! TFs will be empty.\n"; + msg_ = TfData(); + return; + } + // header stamps + msg_.frame_id = "FP_" + tokens.at(from_frame_idx); + msg_.child_frame_id = "FP_" + tokens.at(to_frame_idx); + + msg_.translation = + Vector3ToEigen(tokens.at(translation_x_idx), tokens.at(translation_y_idx), tokens.at(translation_z_idx)); + msg_.rotation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx), + tokens.at(orientation_y_idx), tokens.at(orientation_z_idx)); + + // process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt similarity index 62% rename from fixposition_driver/CMakeLists.txt rename to fixposition_driver_ros1/CMakeLists.txt index 243e73c..07e1898 100644 --- a/fixposition_driver/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -1,6 +1,6 @@ # GENERAL ============================================================================================================== cmake_minimum_required(VERSION 3.5) -project(fixposition_driver LANGUAGES CXX) +project(fixposition_driver_ros1 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 14) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Wall -Wextra -Wpedantic -Wno-unused-parameter") @@ -10,6 +10,8 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # DEPENDENCIES ========================================================================================================= find_package(Boost 1.65.0 REQUIRED) find_package(Eigen3 REQUIRED) +find_package(fixposition_gnss_tf REQUIRED) +find_package(fixposition_driver_lib REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp tf @@ -18,49 +20,55 @@ find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs message_generation - fixposition_gnss_tf + eigen_conversions ) - add_message_files( - FILES - VRTK.msg - Speed.msg + FILES + VRTK.msg + Speed.msg ) generate_messages( - DEPENDENCIES - std_msgs - nav_msgs - geometry_msgs + DEPENDENCIES + std_msgs + nav_msgs + geometry_msgs ) catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS + INCLUDE_DIRS include + CATKIN_DEPENDS tf roscpp geometry_msgs nav_msgs std_msgs message_runtime ) -include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR}) - +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${fixposition_gnss_tf_INCLUDE_DIRS} + ${fixposition_driver_lib_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIR} +) # BUILD EXECUTABLE ===================================================================================================== add_executable( - ${PROJECT_NAME} + ${PROJECT_NAME} src/fixposition_driver_node.cpp - src/fixposition_driver.cpp - src/base_converter.cpp - src/odometry.cpp - src/llh.cpp - src/imu.cpp - src/tf.cpp - src/helper.cpp src/params.cpp - + src/data_to_ros1.cpp +) + +target_link_libraries( + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${fixposition_gnss_tf_LIBRARIES} + ${fixposition_driver_lib_LIBRARIES} + ${Boost_LIBRARIES} + pthread ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} pthread) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) # INSTALL ============================================================================================================== @@ -69,10 +77,10 @@ install(DIRECTORY include/ ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) install(DIRECTORY "launch" diff --git a/fixposition_driver_ros1/Doxyfile b/fixposition_driver_ros1/Doxyfile new file mode 100644 index 0000000..5fdd3e8 --- /dev/null +++ b/fixposition_driver_ros1/Doxyfile @@ -0,0 +1,2427 @@ +# Doxyfile 1.8.11 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Fixposition Driver ROS1" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = doc_gen + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL =YES + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO, these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. +# The default value is: NO. + +WARN_AS_ERROR = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = include src submodules README.md + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl, +# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js. + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = README.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# cost of reduced performance. This can be particularly helpful with template +# rich C++ code for which doxygen's built-in parser lacks the necessary type +# information. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse-libclang=ON option for CMake. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /