diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..5f8688e --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,63 @@ +# Changelog + +_For questions about compatibility, please contact Fixpositions Support support@fixposition.com_ + +## [6.1.3](https://github.com/fixposition/fixposition_driver/releases/tag/6.1.3) + + - Improve README + +## [6.1.2](https://github.com/fixposition/fixposition_driver/releases/tag/6.1.2) + + - Bugfix gnss message timestamps + +## [6.1.1](https://github.com/fixposition/fixposition_driver/releases/tag/6.1.1) + + - Bugfix for serial connection + - Add frame_id for imu messages + +## [6.1.0](https://github.com/fixposition/fixposition_driver/releases/tag/6.1.0) + + - Add feature to publish GNSS Antenna positions even if Fusion is not initialized. NOV_B-BESTGNSSPOS is used and sensor_msgs::NavSatFix is published. See more details in [Vision-RTK2 GNSS Antenna Positions](#Vision-RTK2-GNSS-Antenna-Positions). + +## [6.0.3](https://github.com/fixposition/fixposition_driver/releases/tag/6.0.3) + + - Fix Frame ID of FP_POI in LLH converter + - Bugfix of sending empty TFs + - Fix wrong gnss status + - Remove 'WsCallback' warning + - Add ci file for ROS + +## [6.0.2](https://github.com/fixposition/fixposition_driver/releases/tag/6.0.2) + + - Add missing depencency of `tf2_eigen` in `fixposition_driver_ros2/CMakeList.txt` + +## [6.0.1](https://github.com/fixposition/fixposition_driver/releases/tag/6.0.1) + + - Adapted to be compatible with updated Fixposition message definitions + - **Compatible with Vision-RTK 2 software released after 09.03.2023** + +## [5.0.0](https://github.com/fixposition/fixposition_driver/releases/tag/5.0.0) + + - Support both ROS1 and ROS2 + - Diverse bugfixes in code and documentation + - **This is the last version compatible with Vision-RTK 2 software released before 17.01.2023** + +## [4.4.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.4.0) + + - Pitch-Roll estimation from IMU data parsed from Vision-RTK 2, output available before fusion initialization. + - OdometryConverter imeplemented as an example for wheelspeed data integration. + +## [4.3.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.3.0) + + - Automatic reconnect after connection is lost + - Adaption to latest Vision-RTK 2 software changes + +## [4.2.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.2.0) + + - Euler Angle Yaw-Pitch-Roll in local ENU frame + - Odometry in fixed ENU0 frame + +## [4.0.1](https://github.com/fixposition/fixposition_driver/releases/tag/4.0.1) + + - First public release + - Code cleanup diff --git a/README.md b/README.md index 27407b8..aa1cba0 100644 --- a/README.md +++ b/README.md @@ -10,44 +10,7 @@ The driver is designed to listen on a TCP or Serial port for the [_Fixposition A - 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) -## Changelogs - -_For questions about compatibility, please contact Fixpositions Support support@fixposition.com_ - -- [6.1.1](https://github.com/fixposition/fixposition_driver/releases/tag/6.1.1) - - Bugfix for serial connection - - Add frame_id for imu messages -- [6.1.0](https://github.com/fixposition/fixposition_driver/releases/tag/6.1.0) - - Add feature to publish GNSS Antenna positions even if Fusion is not initialized. NOV_B-BESTGNSSPOS is used and sensor_msgs::NavSatFix is published. See more details in [Vision-RTK2 GNSS Antenna Positions](#Vision-RTK2-GNSS-Antenna-Positions). -- [6.0.3](https://github.com/fixposition/fixposition_driver/releases/tag/6.0.3) - - Fix Frame ID of FP_POI in LLH converter - - Bugfix of sending empty TFs - - Fix wrong gnss status - - Remove 'WsCallback' warning - - Add ci file for ROS -- [6.0.2](https://github.com/fixposition/fixposition_driver/releases/tag/6.0.2) - - Add missing depencency of `tf2_eigen` in `fixposition_driver_ros2/CMakeList.txt` -- [6.0.1](https://github.com/fixposition/fixposition_driver/releases/tag/6.0.1) - - Adapted to be compatible with updated Fixposition message definitions - - **Compatible with Vision-RTK 2 software released after 09.03.2023** -- [5.0.0](https://github.com/fixposition/fixposition_driver/releases/tag/5.0.0) - - Support both ROS1 and ROS2 - - Diverse bugfixes in code and documentation - - **This is the last version compatible with Vision-RTK 2 software released before 17.01.2023** -- [4.4.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.4.0) - - Pitch-Roll estimation from IMU data parsed from Vision-RTK 2, output available before fusion initialization. - - OdometryConverter imeplemented as an example for wheelspeed data integration. -- [4.3.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.3.0) - - Automatic reconnect after connection is lost - - Adaption to latest Vision-RTK 2 software changes -- [4.2.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.2.0) - - Euler Angle Yaw-Pitch-Roll in local ENU frame - - Odometry in fixed ENU0 frame -- [4.0.1](https://github.com/fixposition/fixposition_driver/releases/tag/4.0.1) - - First public release - - Code cleanup - -## Directory structure +## How to use it The code is split in the following 3 parts: @@ -55,6 +18,116 @@ The code is split in the following 3 parts: - `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). +### ROS 1 + +For more details and build instructions, see [here](fixposition_driver_ros1/README.md). + +#### 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 +│ │ ├── fixposition_driver_lib +│ │ ├── fixposition_driver_ros1 +│ │ ├── fixposition_driver_ros2 # will be ignore by catkin +│ ├── fixposition_gnss_tf +``` + +**Add a file named 'CATKIN_IGNORE' to `fixposition_driver_ros2` folder.** + +make sure you have sourced the setup.bash from ros: + +`/opt/ros/{ROS_DISTRO}/setup.bash`, for example + +``` +source /opt/ros/melodic/setup.bash` +``` + +and build it with: + +`catkin build fixposition_driver_ros1` + +This will build the ROS1 driver node and all its dependencies. + +Then source your development environment: + +`source devel/setup.bash` + +#### Launch the Driver + +- To launch the node in serial mode, run: + + `roslaunch fixposition_driver_ros1 serial.launch` + +- In TCP mode (Wi-Fi): + + `roslaunch fixposition_driver_ros1 tcp.launch` + +- In TCP mode (Ethernet): + + `roslaunch fixposition_driver_ros1 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. + +### ROS 2 + +For more details and build instructions, see [here](fixposition_driver_ros2/README.md). + +#### 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 +│ │ ├── fixposition_driver_lib +│ │ ├── fixposition_driver_ros1 # will be ignore by colcon when building for ROS2 +│ │ ├── fixposition_driver_ros2 +│ ├── fixposition_gnss_tf +``` + +**Add a file named 'COLCON_IGNORE' to `fixposition_driver_ros1` folder.** + +make sure you have sourced the setup.bash from ros: + +`/opt/ros/{ROS_DISTRO}/setup.bash`, for example + +``` +source /opt/ros/foxy/setup.bash +``` + +and build it with: + +`colcon build --packages-up-to fixposition_driver_ros2` + +This will build the ROS2 driver node and all its dependencies. + +Then source your environment after the build: + +`source install/setup.bash` + +#### Launch the Driver + +- To launch the node in serial mode, run: + + `ros2 launch fixposition_driver_ros2 serial.launch` + +- In TCP mode (Wi-Fi): + + `ros2 launch fixposition_driver_ros2 tcp.launch` + +- In TCP mode (Ethernet): + + `ros2 launch fixposition_driver_ros2 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. + ## Output of the driver ### Messages and TF tree @@ -110,14 +183,14 @@ The output is published on the following: #### Transforms - TFs: - | Frames | Topic | Message needed to be selected on web-interface | Frequency | + | 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 | - | `FP_POI-->FP_IMU_HORIZONTAL` | `/tf` | `ODOMETRY` | 200Hz | - | `FP_POI-->FP_VRTK` | `/tf_static` | `TF_POI_VRTK` | 1Hz | - | `FP_VRTK-->FP_CAM` | `/tf_static` | `TF_VRTK_CAM` | 1Hz | + | `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 | + | `FP_POI-->FP_IMU_HORIZONTAL` | `/tf` | `ODOMETRY` | 200Hz | + | `FP_POI-->FP_VRTK` | `/tf_static` | `TF_POI_VRTK` | 1Hz | + | `FP_VRTK-->FP_CAM` | `/tf_static` | `TF_VRTK_CAM` | 1Hz | - ROS TF Tree: @@ -155,29 +228,29 @@ The input velocity values should be in [mm/s], respectively [mrad/s], as integer 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) | +| # | 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`): @@ -261,52 +334,52 @@ Example message (wrapped on multiple lines for readability): Message fields: -| # | Field | Format | Unit | Example | Description | -| ---: | -------------------- | ---------- | --------------------------- | --------------------------- | ---------------------------------------------------------------------- | -| 1 | `msg_type` | String | - | `ODOMETRY` | Message type, always `ODOMETRY` for this message | -| 2 | `msg_version` | Numeric | - | `2` | Message version, always `2` for this version of the `ODOMETRY` message | -| 3 | `gps_week` | Numeric | - | `2231` | GPS week number, range 0--9999 | -| 4 | `gps_tow` | Float (.6) | s | `227610.750000` | GPS time of week, range 0.000--604799.999999 | -| 5 | `pos_x` | Float (.4) | m | `4279243.1641` | Position in ECEF, X component | -| 6 | `pos_y` | Float (.4) | m | `635824.2171` | Position in ECEF, Y component | -| 7 | `pos_z` | Float (.4) | m | `4671589.8683` | Position in ECEF, Z component | -| 8 | `orientation_w` | Float (.6) | - | `-0.412792` | Quaternion with respect to ECEF, W component | -| 9 | `orientation_x` | Float (.6) | - | `0.290804` | Quaternion with respect to ECEF, X component | -| 10 | `orientation_y` | Float (.6) | - | `-0.123898` | Quaternion with respect to ECEF, Y component | -| 11 | `orientation_z` | Float (.6) | - | `0.854216` | Quaternion with respect to ECEF, Z component | -| 12 | `vel_x` | Float (.4) | m/s | `-17.1078` | Velocity in output frame, X component | -| 13 | `vel_y` | Float (.4) | m/s | `-0.0526` | Velocity in output frame, Y component | -| 14 | `vel_z` | Float (.4) | m/s | `-0.3252` | Velocity in output frame, Z component | -| 15 | `rot_x` | Float (.5) | rad/s | `0.02245` | Bias corrected angular velocity in output frame, X component | -| 16 | `rot_y` | Float (.5) | rad/s | `0.00275` | Bias corrected angular velocity in output frame, Y component | -| 17 | `rot_z` | Float (.5) | rad/s | `0.10369` | Bias corrected angular velocity in output frame, Z component | -| 18 | `acc_x` | Float (.4) | m/s2 | `-1.0385` | Bias corrected acceleration in output frame, X component | -| 19 | `acc_y` | Float (.4) | m/s2 | `-1.3707` | Bias corrected acceleration in output frame, Y component | -| 20 | `acc_z` | Float (.4) | m/s2 | `9.8249` | Bias corrected acceleration in output frame, Z component | -| 21 | `fusion_status` | Numeric | - | `4` | Fustion status, see below | -| 22 | `imu_bias_status` | Numeric | - | `1` | IMU bias status, see below | -| 23 | `gnss1_fix` | Numeric | - | `8` | Fix status of GNSS1 receiver, see below | -| 24 | `gnss2_fix` | Numeric | - | `8` | Fix status of GNSS2 receiver, see below | -| 25 | `wheelspeed_status` | Numeric | - | `1` | Wheelspeed status, see below | -| 26 | `pos_cov_xx` | Float (5) | m2 | `0.01761` | Position covariance, element XX | -| 27 | `pos_cov_yy` | Float (5) | m2 | `0.02274` | Position covariance, element YY | -| 28 | `pos_cov_zz` | Float (5) | m2 | `0.01713` | Position covariance, element ZZ | -| 29 | `pos_cov_xy` | Float (5) | m2 | `-0.00818` | Position covariance, element XY | -| 30 | `pos_cov_yz` | Float (5) | m2 | `0.00235` | Position covariance, element YZ | -| 31 | `pos_cov_xz` | Float (5) | m2 | `0.00129` | Position covariance, element XZ | -| 32 | `orientation_cov_xx` | Float (5) | rad2 | `0.00013` | Velocity covariance, element XX | -| 33 | `orientation_cov_yy` | Float (5) | rad2 | `0.00015` | Velocity covariance, element YY | -| 34 | `orientation_cov_zz` | Float (5) | rad2 | `0.00014` | Velocity covariance, element ZZ | -| 35 | `orientation_cov_xy` | Float (5) | rad2 | `-0.00001` | Velocity covariance, element XY | -| 36 | `orientation_cov_yz` | Float (5) | rad2 | `0.00001` | Velocity covariance, element YZ | -| 37 | `orientation_cov_xz` | Float (5) | rad2 | `0.00002` | Velocity covariance, element XZ | -| 38 | `vel_cov_xx` | Float (5) | m2/s2 | `0.03482` | Velocity covariance, element XX | -| 39 | `vel_cov_yy` | Float (5) | m2/s2 | `0.06244` | Velocity covariance, element YY | -| 40 | `vel_cov_zz` | Float (5) | m2/s2 | `0.05480` | Velocity covariance, element ZZ | -| 41 | `vel_cov_xy` | Float (5) | m2/s2 | `0.00096` | Velocity covariance, element XY | -| 42 | `vel_cov_yz` | Float (5) | m2/s2 | `0.00509` | Velocity covariance, element YZ | -| 43 | `vel_cov_xz` | Float (5) | m2/s2 | `0.00054` | Velocity covariance, element XZ | -| 44 | `sw_version` | String | - | `fp_release_vr2_2.54.0_160` | Software version | +| # | Field | Format | Unit | Example | Description | +| --: | -------------------- | ---------- | --------------------------- | --------------------------- | ---------------------------------------------------------------------- | +| 1 | `msg_type` | String | - | `ODOMETRY` | Message type, always `ODOMETRY` for this message | +| 2 | `msg_version` | Numeric | - | `2` | Message version, always `2` for this version of the `ODOMETRY` message | +| 3 | `gps_week` | Numeric | - | `2231` | GPS week number, range 0--9999 | +| 4 | `gps_tow` | Float (.6) | s | `227610.750000` | GPS time of week, range 0.000--604799.999999 | +| 5 | `pos_x` | Float (.4) | m | `4279243.1641` | Position in ECEF, X component | +| 6 | `pos_y` | Float (.4) | m | `635824.2171` | Position in ECEF, Y component | +| 7 | `pos_z` | Float (.4) | m | `4671589.8683` | Position in ECEF, Z component | +| 8 | `orientation_w` | Float (.6) | - | `-0.412792` | Quaternion with respect to ECEF, W component | +| 9 | `orientation_x` | Float (.6) | - | `0.290804` | Quaternion with respect to ECEF, X component | +| 10 | `orientation_y` | Float (.6) | - | `-0.123898` | Quaternion with respect to ECEF, Y component | +| 11 | `orientation_z` | Float (.6) | - | `0.854216` | Quaternion with respect to ECEF, Z component | +| 12 | `vel_x` | Float (.4) | m/s | `-17.1078` | Velocity in output frame, X component | +| 13 | `vel_y` | Float (.4) | m/s | `-0.0526` | Velocity in output frame, Y component | +| 14 | `vel_z` | Float (.4) | m/s | `-0.3252` | Velocity in output frame, Z component | +| 15 | `rot_x` | Float (.5) | rad/s | `0.02245` | Bias corrected angular velocity in output frame, X component | +| 16 | `rot_y` | Float (.5) | rad/s | `0.00275` | Bias corrected angular velocity in output frame, Y component | +| 17 | `rot_z` | Float (.5) | rad/s | `0.10369` | Bias corrected angular velocity in output frame, Z component | +| 18 | `acc_x` | Float (.4) | m/s2 | `-1.0385` | Bias corrected acceleration in output frame, X component | +| 19 | `acc_y` | Float (.4) | m/s2 | `-1.3707` | Bias corrected acceleration in output frame, Y component | +| 20 | `acc_z` | Float (.4) | m/s2 | `9.8249` | Bias corrected acceleration in output frame, Z component | +| 21 | `fusion_status` | Numeric | - | `4` | Fustion status, see below | +| 22 | `imu_bias_status` | Numeric | - | `1` | IMU bias status, see below | +| 23 | `gnss1_fix` | Numeric | - | `8` | Fix status of GNSS1 receiver, see below | +| 24 | `gnss2_fix` | Numeric | - | `8` | Fix status of GNSS2 receiver, see below | +| 25 | `wheelspeed_status` | Numeric | - | `1` | Wheelspeed status, see below | +| 26 | `pos_cov_xx` | Float (5) | m2 | `0.01761` | Position covariance, element XX | +| 27 | `pos_cov_yy` | Float (5) | m2 | `0.02274` | Position covariance, element YY | +| 28 | `pos_cov_zz` | Float (5) | m2 | `0.01713` | Position covariance, element ZZ | +| 29 | `pos_cov_xy` | Float (5) | m2 | `-0.00818` | Position covariance, element XY | +| 30 | `pos_cov_yz` | Float (5) | m2 | `0.00235` | Position covariance, element YZ | +| 31 | `pos_cov_xz` | Float (5) | m2 | `0.00129` | Position covariance, element XZ | +| 32 | `orientation_cov_xx` | Float (5) | rad2 | `0.00013` | Velocity covariance, element XX | +| 33 | `orientation_cov_yy` | Float (5) | rad2 | `0.00015` | Velocity covariance, element YY | +| 34 | `orientation_cov_zz` | Float (5) | rad2 | `0.00014` | Velocity covariance, element ZZ | +| 35 | `orientation_cov_xy` | Float (5) | rad2 | `-0.00001` | Velocity covariance, element XY | +| 36 | `orientation_cov_yz` | Float (5) | rad2 | `0.00001` | Velocity covariance, element YZ | +| 37 | `orientation_cov_xz` | Float (5) | rad2 | `0.00002` | Velocity covariance, element XZ | +| 38 | `vel_cov_xx` | Float (5) | m2/s2 | `0.03482` | Velocity covariance, element XX | +| 39 | `vel_cov_yy` | Float (5) | m2/s2 | `0.06244` | Velocity covariance, element YY | +| 40 | `vel_cov_zz` | Float (5) | m2/s2 | `0.05480` | Velocity covariance, element ZZ | +| 41 | `vel_cov_xy` | Float (5) | m2/s2 | `0.00096` | Velocity covariance, element XY | +| 42 | `vel_cov_yz` | Float (5) | m2/s2 | `0.00509` | Velocity covariance, element YZ | +| 43 | `vel_cov_xz` | Float (5) | m2/s2 | `0.00054` | Velocity covariance, element XZ | +| 44 | `sw_version` | String | - | `fp_release_vr2_2.54.0_160` | Software version | Fusion status (`fusion_status`): @@ -364,21 +437,21 @@ Example message (wrapped on multiple lines for readability): Message fields: -| # | Field | Format | Unit | Example | Description | -| ---: | ------------- | ---------- | ------------- | --------------- | -------------------------------------------------------------------------- | -| 1 | `msg_type` | String | - | `LLH` | Message type, always `LLH` for this message | -| 2 | `msg_version` | Numeric | - | `1` | Message version, always `1` for this version of the `LLH` message | -| 3 | `gps_week` | Numeric | - | `2231` | GPS week number, range 0--9999 | -| 4 | `gps_tow` | Float (.6) | s | `227563.250000` | GPS time of week, range 0.000--604799.999999 | -| 5 | `latitude` | Float (.9) | deg | `47.392357470` | Latitude, range -90.000000000--90.000000000, > 0 for North, < 0 for South | -| 6 | `longitude` | Float (.9) | deg | `8.448121451` | Longitude, range -180.000000000--180.000000000, > 0 for East, < 0 for West | -| 7 | `height` | Float (.4) | m | `473.5857` | Ellipsoidal height, range -1000.0000-50000.0000 | -| 8 | `pos_cov_ee` | Float (5) | m2 | `0.04533` | Position covariance in ENU, element EE | -| 9 | `pos_cov_nn` | Float (5) | m2 | `0.03363` | Position covariance in ENU, element NN | -| 10 | `pos_cov_uu` | Float (5) | m2 | `0.02884` | Position covariance in ENU, element UU | -| 11 | `pos_cov_en` | Float (5) | m2 | `0.00417` | Position covariance in ENU, element EN | -| 12 | `pos_cov_nu` | Float (5) | m2 | `0.00086` | Position covariance in ENU, element NU | -| 13 | `pos_cov_eu` | Float (5) | m2 | `-0.00136` | Position covariance in ENU, element EU | +| # | Field | Format | Unit | Example | Description | +| --: | ------------- | ---------- | ------------- | --------------- | -------------------------------------------------------------------------- | +| 1 | `msg_type` | String | - | `LLH` | Message type, always `LLH` for this message | +| 2 | `msg_version` | Numeric | - | `1` | Message version, always `1` for this version of the `LLH` message | +| 3 | `gps_week` | Numeric | - | `2231` | GPS week number, range 0--9999 | +| 4 | `gps_tow` | Float (.6) | s | `227563.250000` | GPS time of week, range 0.000--604799.999999 | +| 5 | `latitude` | Float (.9) | deg | `47.392357470` | Latitude, range -90.000000000--90.000000000, > 0 for North, < 0 for South | +| 6 | `longitude` | Float (.9) | deg | `8.448121451` | Longitude, range -180.000000000--180.000000000, > 0 for East, < 0 for West | +| 7 | `height` | Float (.4) | m | `473.5857` | Ellipsoidal height, range -1000.0000-50000.0000 | +| 8 | `pos_cov_ee` | Float (5) | m2 | `0.04533` | Position covariance in ENU, element EE | +| 9 | `pos_cov_nn` | Float (5) | m2 | `0.03363` | Position covariance in ENU, element NN | +| 10 | `pos_cov_uu` | Float (5) | m2 | `0.02884` | Position covariance in ENU, element UU | +| 11 | `pos_cov_en` | Float (5) | m2 | `0.00417` | Position covariance in ENU, element EN | +| 12 | `pos_cov_nu` | Float (5) | m2 | `0.00086` | Position covariance in ENU, element NU | +| 13 | `pos_cov_eu` | Float (5) | m2 | `-0.00136` | Position covariance in ENU, element EU | ### RAWIMU message @@ -393,18 +466,18 @@ Example message: Message fields: -| # | Field | Format | Unit | Example | Description | -| ---: | ------------- | ---------- | --------------- | --------------- | -------------------------------------------------------------------- | -| 1 | `msg_type` | String | - | `RAWIMU` | Message type, always `RAWIMU` for this message | -| 2 | `msg_version` | Numeric | - | `1` | Message version, always `1` for this version of the `RAWIMU` message | -| 3 | `gps_week` | Numeric | - | `2197` | GPS week number, range 0--9999 | -| 4 | `gps_tow` | Float (.6) | s | `126191.777855` | GPS time of week, range 0.000--604799.999999 | -| 5 | `acc_x` | Float (.6) | m/s2 | `-0.199914` | Raw acceleration in output frame, X component | -| 6 | `acc_y` | Float (.6) | m/s2 | `0.472851` | Raw acceleration in output frame, Y component | -| 7 | `acc_z` | Float (.6) | m/s2 | `9.917973` | Raw acceleration in output frame, Z component | -| 8 | `rot_x` | Float (.6) | rad/s | `0.023436` | Raw angular velocity in output frame, X component | -| 9 | `rot_y` | Float (.6) | rad/s | `0.007723` | Raw angular velocity in output frame, Y component | -| 10 | `rot_z` | Float (.6) | rad/s | `0.002131` | Raw angular velocity in output frame, Z component | +| # | Field | Format | Unit | Example | Description | +| --: | ------------- | ---------- | --------------- | --------------- | -------------------------------------------------------------------- | +| 1 | `msg_type` | String | - | `RAWIMU` | Message type, always `RAWIMU` for this message | +| 2 | `msg_version` | Numeric | - | `1` | Message version, always `1` for this version of the `RAWIMU` message | +| 3 | `gps_week` | Numeric | - | `2197` | GPS week number, range 0--9999 | +| 4 | `gps_tow` | Float (.6) | s | `126191.777855` | GPS time of week, range 0.000--604799.999999 | +| 5 | `acc_x` | Float (.6) | m/s2 | `-0.199914` | Raw acceleration in output frame, X component | +| 6 | `acc_y` | Float (.6) | m/s2 | `0.472851` | Raw acceleration in output frame, Y component | +| 7 | `acc_z` | Float (.6) | m/s2 | `9.917973` | Raw acceleration in output frame, Z component | +| 8 | `rot_x` | Float (.6) | rad/s | `0.023436` | Raw angular velocity in output frame, X component | +| 9 | `rot_y` | Float (.6) | rad/s | `0.007723` | Raw angular velocity in output frame, Y component | +| 10 | `rot_z` | Float (.6) | rad/s | `0.002131` | Raw angular velocity in output frame, Z component | ### CORRIMU message @@ -419,18 +492,18 @@ Example message: Message fields: -| # | Field | Format | Unit | Example | Description | -| ---: | ------------- | ---------- | --------------- | --------------- | -------------------------------------------------------------------- | -| 1 | `msg_type` | String | - | `CORRIMU` | Message type, always `RAWIMU` for this message | -| 2 | `msg_version` | Numeric | - | `1` | Message version, always `1` for this version of the `RAWIMU` message | -| 3 | `gps_week` | Numeric | - | `2197` | GPS week number, range 0--9999 | -| 4 | `gps_tow` | Float (.6) | s | `126191.777855` | GPS time of week, range 0.000--604799.999 | -| 5 | `acc_x` | Float (.6) | m/s2 | `-0.195224` | Raw acceleration in output frame, X component | -| 6 | `acc_y` | Float (.6) | m/s2 | `0.393969` | Raw acceleration in output frame, Y component | -| 7 | `acc_z` | Float (.6) | m/s2 | `9.869998` | Raw acceleration in output frame, Z component | -| 8 | `rot_x` | Float (.6) | rad/s | `0.013342` | Raw angular velocity in output frame, X component | -| 9 | `rot_y` | Float (.6) | rad/s | `-0.004620` | Raw angular velocity in output frame, Y component | -| 10 | `rot_z` | Float (.6) | rad/s | `-0.000728` | Raw angular velocity in output frame, Z component | +| # | Field | Format | Unit | Example | Description | +| --: | ------------- | ---------- | --------------- | --------------- | -------------------------------------------------------------------- | +| 1 | `msg_type` | String | - | `CORRIMU` | Message type, always `RAWIMU` for this message | +| 2 | `msg_version` | Numeric | - | `1` | Message version, always `1` for this version of the `RAWIMU` message | +| 3 | `gps_week` | Numeric | - | `2197` | GPS week number, range 0--9999 | +| 4 | `gps_tow` | Float (.6) | s | `126191.777855` | GPS time of week, range 0.000--604799.999 | +| 5 | `acc_x` | Float (.6) | m/s2 | `-0.195224` | Raw acceleration in output frame, X component | +| 6 | `acc_y` | Float (.6) | m/s2 | `0.393969` | Raw acceleration in output frame, Y component | +| 7 | `acc_z` | Float (.6) | m/s2 | `9.869998` | Raw acceleration in output frame, Z component | +| 8 | `rot_x` | Float (.6) | rad/s | `0.013342` | Raw angular velocity in output frame, X component | +| 9 | `rot_y` | Float (.6) | rad/s | `-0.004620` | Raw angular velocity in output frame, Y component | +| 10 | `rot_z` | Float (.6) | rad/s | `-0.000728` | Raw angular velocity in output frame, Z component | Remarks: @@ -448,21 +521,21 @@ Example messages: Message fields: -| # | Field | Format | Unit | Example | Description | -| ---: | --------------- | ---------- | ---- | --------------- | ---------------------------------------------------------------- | -| 1 | `msg_type` | String | - | `TF` | Message type, always `TF` for this message | -| 2 | `msg_version` | Numeric | - | `2` | Message version, always `2` for this version of the `TF` message | -| 3 | `gps_week` | Numeric | - | `2233` | GPS week number, range 0--9999 | -| 4 | `gps_tow` | Float (.6) | s | `315835.000000` | GPS time of week, range 0.000--604799.999999 | -| 5 | `frame_a` | String | - | `POI` | Target frame (maximum 8 characters: A-Z and 0-9) | -| 6 | `frame_b` | String | - | `VRTK` | Initial frame (maximum 8 characters: A-Z and 0-9) | -| 7 | `translation_x` | Float (.5) | m | `-0.99301` | Translation, X component | -| 8 | `translation_y` | Float (.5) | m | `-2.01395` | Translation, Y component | -| 9 | `translation_z` | Float (.5) | m | `-2.99298` | Translation, Z component | -| 10 | `orientation_w` | Float (.6) | - | `0.999995` | Rotation in quaternion, W component | -| 11 | `orientation_x` | Float (.6) | - | `-0.002616` | Rotation in quaternion, X component | -| 12 | `orientation_y` | Float (.6) | - | `-0.001748` | Rotation in quaternion, Y component | -| 13 | `orientation_z` | Float (.6) | - | `-0.000868` | Rotation in quaternion, Z component | +| # | Field | Format | Unit | Example | Description | +| --: | --------------- | ---------- | ---- | --------------- | ---------------------------------------------------------------- | +| 1 | `msg_type` | String | - | `TF` | Message type, always `TF` for this message | +| 2 | `msg_version` | Numeric | - | `2` | Message version, always `2` for this version of the `TF` message | +| 3 | `gps_week` | Numeric | - | `2233` | GPS week number, range 0--9999 | +| 4 | `gps_tow` | Float (.6) | s | `315835.000000` | GPS time of week, range 0.000--604799.999999 | +| 5 | `frame_a` | String | - | `POI` | Target frame (maximum 8 characters: A-Z and 0-9) | +| 6 | `frame_b` | String | - | `VRTK` | Initial frame (maximum 8 characters: A-Z and 0-9) | +| 7 | `translation_x` | Float (.5) | m | `-0.99301` | Translation, X component | +| 8 | `translation_y` | Float (.5) | m | `-2.01395` | Translation, Y component | +| 9 | `translation_z` | Float (.5) | m | `-2.99298` | Translation, Z component | +| 10 | `orientation_w` | Float (.6) | - | `0.999995` | Rotation in quaternion, W component | +| 11 | `orientation_x` | Float (.6) | - | `-0.002616` | Rotation in quaternion, X component | +| 12 | `orientation_y` | Float (.6) | - | `-0.001748` | Rotation in quaternion, Y component | +| 13 | `orientation_z` | Float (.6) | - | `-0.000868` | Rotation in quaternion, Z component | # Fixposition Odometry Converter