From 8079811e98bc46d94b62d0b35c7a20c660531a5d Mon Sep 17 00:00:00 2001 From: Kevin Greene Date: Wed, 16 Aug 2023 01:39:47 -0700 Subject: [PATCH 01/15] Fix include paths for ROS2 headers (#37) * Fix include paths for ROS2 headers * Add conditional include on deprecated header --- .../fixposition_driver_ros2/fixposition_driver_node.hpp | 6 +++--- fixposition_driver_ros2/src/data_to_ros2.cpp | 5 +++++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index a75cb81..20a3875 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -21,10 +21,10 @@ #include /* ROS2 */ -#include +#include #include -#include -#include +#include +#include #include #include #include diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 36272c0..14f626d 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -13,7 +13,12 @@ */ /* ROS */ +#if __has_include() +#include +#else +// This header was deprecated as of ROS2 Humble, but is still required in order to support Foxy. #include +#endif /* PACKAGE */ #include From 366abf3afe73f8dc70b86ab8e04038a9cd997d3e Mon Sep 17 00:00:00 2001 From: fixposition-support Date: Tue, 5 Sep 2023 11:07:14 +0200 Subject: [PATCH 02/15] Update README regarding YAML loading (#38) * Update readme --- README.md | 290 ++++++++++++++++++++++++++++-------------------------- 1 file changed, 150 insertions(+), 140 deletions(-) diff --git a/README.md b/README.md index 75f0be3..070bb80 100644 --- a/README.md +++ b/README.md @@ -126,7 +126,17 @@ Then source your environment after the build: `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. +**To change the settings of TCP (IP, Port) or Serial (Baudrate, Port) connections, check the `launch/tcp.yaml` and `launch/serial.yaml` files and read this note below.** + +> [!NOTE] +> ROS2, unlike ROS1, by default uses a `install` directory in the workspace. So when you do `ros2 launch xxx`, the configuration and launch files are taken from the `install` and not directly from the `src` directory. +> +> If you want to modify the parameters in the YAML files. You can: +> - Modify the YAML file in the `src` directory and then re-run `colcon build --packages-up-to fixposition_driver_ros2` to update them into the `install` directory. +> +> or +> - Modify the YAML file in `install`. However, the next time you do `colcon build` they will be overriden by the files in `src`. + ## Output of the driver @@ -183,14 +193,14 @@ The output is published on the following: #### Transforms - 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 | - | `FP_POI-->FP_IMUH` | `/tf` | `ODOMETRY` | 200Hz | - | `FP_POI-->FP_VRTK` | `/tf_static` | `TF_POI_VRTK` | 1Hz | - | `FP_VRTK-->FP_CAM` | `/tf_static` | `TF_VRTK_CAM` | 1Hz | + | 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_IMUH` | `/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: @@ -206,14 +216,14 @@ _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. | +| 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_IMUH** | 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 @@ -228,29 +238,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`): @@ -334,52 +344,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`): @@ -437,21 +447,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 @@ -466,18 +476,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 @@ -492,18 +502,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: @@ -521,21 +531,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 From 38eb997ef0b4aa1017926fa95214ecb225af7462 Mon Sep 17 00:00:00 2001 From: fgarciacardenas <47540719+fgarciacardenas@users.noreply.github.com> Date: Wed, 25 Oct 2023 11:44:17 +0200 Subject: [PATCH 03/15] Updated Fixposition Odometry Converter readme (#40) Co-authored-by: Facundo Garcia --- fixposition_odometry_converter/README.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/fixposition_odometry_converter/README.md b/fixposition_odometry_converter/README.md index c3780a8..6095c03 100644 --- a/fixposition_odometry_converter/README.md +++ b/fixposition_odometry_converter/README.md @@ -1,8 +1,6 @@ # Fixposition Odometry Converter -_This is currently only available in ROS1, a ROS2 version will be release later._ - ## Description 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 `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. 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. If the param `use_angular` is selected, the z component of the angular velocity (yaw rate) of the input messages is also extracted and placed in the speed vector. From c4440d90bc686dd14542dfe8d713ae3b9fc10e31 Mon Sep 17 00:00:00 2001 From: fgarciacardenas <47540719+fgarciacardenas@users.noreply.github.com> Date: Wed, 6 Dec 2023 10:11:17 +0100 Subject: [PATCH 04/15] Added Vector3Stamped to ypr and imu_ypr topics in ROS1 (#42) * Added Vector3Stamped to ypr and imu_ypr topics in ROS1 * Added ROS2 changes --------- Co-authored-by: Facundo Garcia --- .../fixposition_driver_ros1/data_to_ros1.hpp | 1 + .../src/fixposition_driver_node.cpp | 16 ++++++++----- .../fixposition_driver_ros2/data_to_ros2.hpp | 1 + .../fixposition_driver_node.hpp | 4 ++-- .../src/fixposition_driver_node.cpp | 24 +++++++++++-------- 5 files changed, 28 insertions(+), 18 deletions(-) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 5ebd542..0bb259f 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -16,6 +16,7 @@ #define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__ /* ROS */ #include +#include #include #include #include diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index f834991..49a9225 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -46,8 +46,8 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para poiimu_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)) { + eul_pub_(nh_.advertise("/fixposition/ypr", 100)), + eul_imu_pub_(nh_.advertise("/fixposition/imu_ypr", 100)) { ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 100, &FixpositionDriverNode::WsCallback, this, ros::TransportHints().tcpNoDelay()); @@ -85,8 +85,10 @@ void FixpositionDriverNode::RegisterObservers() { vrtk_pub_.publish(vrtk); } if (eul_pub_.getNumSubscribers() > 0) { - geometry_msgs::Vector3 ypr; - tf::vectorEigenToMsg(data.eul, ypr); + geometry_msgs::Vector3Stamped ypr; + ypr.header.stamp = ros::Time::fromBoost(fixposition::times::GpsTimeToPtime(data.odometry.stamp)); + ypr.header.frame_id = "FP_POI"; + tf::vectorEigenToMsg(data.eul, ypr.vector); eul_pub_.publish(ypr); } @@ -142,8 +144,10 @@ void FixpositionDriverNode::RegisterObservers() { // Publish Pitch Roll based on IMU only Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone - geometry_msgs::Vector3 imu_ypr; - tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr); + geometry_msgs::Vector3Stamped imu_ypr; + imu_ypr.header.stamp = tf.header.stamp; + imu_ypr.header.frame_id = "FP_POI"; + tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector); eul_imu_pub_.publish(imu_ypr); } else { diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index e429924..0bcdc16 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 20a3875..609ca39 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -76,8 +76,8 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message rclcpp::Publisher::SharedPtr odometry_enu0_pub_; //!< ENU0 Odometry - rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU + rclcpp::Publisher::SharedPtr eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in // local horizontal diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index edf25bd..0046534 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -47,8 +47,8 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, poiimu_pub_(node_->create_publisher("/fixposition/poiimu", 100)), vrtk_pub_(node_->create_publisher("/fixposition/vrtk", 100)), odometry_enu0_pub_(node_->create_publisher("/fixposition/odometry_enu", 100)), - eul_pub_(node_->create_publisher("/fixposition/ypr", 100)), - eul_imu_pub_(node_->create_publisher("/fixposition/imu_ypr", 100)), + eul_pub_(node_->create_publisher("/fixposition/ypr", 100)), + eul_imu_pub_(node_->create_publisher("/fixposition/imu_ypr", 100)), br_(std::make_shared(node_)), static_br_(std::make_shared(node_)) { ws_sub_ = node_->create_subscription( @@ -110,10 +110,12 @@ void FixpositionDriverNode::RegisterObservers() { vrtk_pub_->publish(vrtk); } if (eul_pub_->get_subscription_count() > 0) { - geometry_msgs::msg::Vector3 ypr; - ypr.set__x(data.eul.x()); - ypr.set__y(data.eul.y()); - ypr.set__z(data.eul.z()); + geometry_msgs::msg::Vector3Stamped ypr; + ypr.header.stamp = GpsTimeToMsgTime(data.odometry.stamp); + ypr.header.frame_id = "FP_POI"; + ypr.vector.set__x(data.eul.x()); + ypr.vector.set__y(data.eul.y()); + ypr.vector.set__z(data.eul.z()); eul_pub_->publish(ypr); } @@ -169,10 +171,12 @@ void FixpositionDriverNode::RegisterObservers() { // Publish Pitch Roll based on IMU only Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone - geometry_msgs::msg::Vector3 imu_ypr; - imu_ypr.set__x(imu_ypr_eigen.x()); - imu_ypr.set__y(imu_ypr_eigen.y()); - imu_ypr.set__z(imu_ypr_eigen.z()); + geometry_msgs::msg::Vector3Stamped imu_ypr; + imu_ypr.header.stamp = tf.header.stamp; + imu_ypr.header.frame_id = "FP_POI"; + imu_ypr.vector.set__x(imu_ypr_eigen.x()); + imu_ypr.vector.set__y(imu_ypr_eigen.y()); + imu_ypr.vector.set__z(imu_ypr_eigen.z()); eul_imu_pub_->publish(imu_ypr); } else { From 021b0f205b1f88b934b2a7edd1d4d21aa26c8513 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 11 Jan 2024 10:32:11 +0100 Subject: [PATCH 05/15] Added gpgga to ROS1 --- fixposition_driver_lib/CMakeLists.txt | 1 + .../converter/gpgga.hpp | 65 ++++++++++++++++ .../src/fixposition_driver.cpp | 15 +++- fixposition_driver_lib/src/gpgga.cpp | 75 +++++++++++++++++++ .../fixposition_driver_node.hpp | 1 + fixposition_driver_ros1/launch/tcp.yaml | 2 +- .../src/fixposition_driver_node.cpp | 11 +++ fixposition_driver_ros2/CATKIN_IGNORE | 0 .../fixposition_driver_node.hpp | 1 + .../src/fixposition_driver_node.cpp | 11 +++ 10 files changed, 179 insertions(+), 3 deletions(-) create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp create mode 100644 fixposition_driver_lib/src/gpgga.cpp create mode 100644 fixposition_driver_ros2/CATKIN_IGNORE diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 6df5040..859d3c3 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -29,6 +29,7 @@ add_library( src/tf.cpp src/helper.cpp src/parser.cpp + src/gpgga.cpp ) target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp new file mode 100644 index 0000000..83fbb3a --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp @@ -0,0 +1,65 @@ +/** + * @file + * @brief Declaration of GpggaConverter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include +#include +#include + +namespace fixposition { + +class GpggaConverter : public BaseAsciiConverter { + public: + using GpggaObserver = std::function; + /** + * @brief Construct a new GpggaConverter + * + */ + GpggaConverter() {} + + ~GpggaConverter() = default; + + /** + * @brief Take comma-delimited tokens of GPGGA message, convert to Data structs and if available, + * call observers + * Example: + * $GPGGA,151229.40,4723.54108,N,00826.88485,E,4,12,00.98,473.5,M,,,,*3A\r\n + * + * @param[in] tokens message split in tokens + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(GpggaObserver ob) { obs_.push_back(ob); } + + private: + NavSatFixData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kVersion_ = 1; + static constexpr const int kSize_ = 14; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 3901539..a53711c 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,8 @@ bool FixpositionDriver::InitializeConverters() { a_converters_["RAWIMU"] = std::unique_ptr(new ImuConverter(false)); } else if (format == "CORRIMU") { a_converters_["CORRIMU"] = std::unique_ptr(new ImuConverter(true)); + } else if (format == "GPGGA") { + a_converters_["GPGGA"] = std::unique_ptr(new GpggaConverter()); } else if (format == "TF") { if (a_converters_.find("TF") == a_converters_.end()) { a_converters_["TF"] = std::unique_ptr(new TfConverter()); @@ -230,12 +233,20 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); // if it doesn't start with FP then do nothing - if (tokens.at(0) != "FP") { + if ((tokens.at(0) != "FP") || (tokens.at(0) != "GPGGA")) { return; } // Get the header of the sentence - const std::string header = tokens.at(1); + std::string _header; + if (tokens.at(0) == "GPGGA") { + _header = "GPGGA"; + } else { + _header = tokens.at(1); + } + const std::string header = _header; + std::cout << "Header: " << _header << std::endl; + std::cout << "Header: " << header << std::endl; // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU" if (a_converters_[header] != nullptr) { diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp new file mode 100644 index 0000000..bfeb29a --- /dev/null +++ b/fixposition_driver_lib/src/gpgga.cpp @@ -0,0 +1,75 @@ +/** + * @file + * @brief Implementation of GpggaConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int time_idx = 1; +static constexpr const int lat_idx = 2; +static constexpr const int lat_ns_idx = 3; +static constexpr const int lon_idx = 4; +static constexpr const int lon_ew_idx = 5; +static constexpr const int quality_idx = 6; +static constexpr const int num_sv_idx = 7; +static constexpr const int hdop_idx = 8; +static constexpr const int alt_idx = 9; +static constexpr const int alt_unit_idx = 10; +static constexpr const int sep_idx = 11; +static constexpr const int sep_unit_idx = 12; +static constexpr const int diff_age_idx = 13; +static constexpr const int diff_sta_idx = 14; + +void GpggaConverter::ConvertTokens(const std::vector& tokens) { + // Check if message size is wrong + bool ok = tokens.size() == kSize_; + if (!ok) { + std::cout << "Error in parsing GPGGA string with " << tokens.size() << " fields! GPGGA message will be empty.\n"; + msg_ = NavSatFixData(); + return; + } + + // Header stamps + msg_.stamp = ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx)); + msg_.frame_id = "LLH"; + + // LLH coordinates + msg_.latitude = StringToDouble(tokens.at(lat_idx)); + msg_.longitude = StringToDouble(tokens.at(lon_idx)); + msg_.altitude = StringToDouble(tokens.at(alt_idx)); + + // Covariance diagonals + const double hdop = StringToDouble(tokens.at(hdop_idx)); + msg_.cov(0, 0) = hdop * hdop; + msg_.cov(1, 1) = hdop * hdop; + msg_.cov(2, 2) = 4 * hdop * hdop; + + // Rest of covariance fields + msg_.cov(0, 1) = msg_.cov(1, 0) = 0.0; + msg_.cov(0, 2) = msg_.cov(2, 0) = 0.0; + msg_.cov(1, 2) = msg_.cov(2, 1) = 0.0; + msg_.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 064ea13..c58625e 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -69,6 +69,7 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher navsatfix_pub_; ros::Publisher navsatfix_gnss1_pub_; ros::Publisher navsatfix_gnss2_pub_; + ros::Publisher navsatfix_gpgga_pub_; ros::Publisher odometry_pub_; //!< ECEF Odometry ros::Publisher poiimu_pub_; //!< Bias corrected IMU ros::Publisher vrtk_pub_; //!< VRTK message diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index f9165b6..9d535e2 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -1,5 +1,5 @@ fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"] + formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "TF"] type: tcp port: "21000" ip: "10.0.2.1" # change to VRTK2's IP address in the network diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 49a9225..adb646b 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para navsatfix_pub_(nh_.advertise("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(nh_.advertise("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(nh_.advertise("/fixposition/gnss2", 100)), + navsatfix_gpgga_pub_(nh_.advertise("/fixposition/gpgga", 100)), // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), @@ -154,6 +156,15 @@ void FixpositionDriverNode::RegisterObservers() { static_br_.sendTransform(tf); } }); + } else if (format == "GPGGA") { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) { + // GPGGA Observer Lambda + if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) { + sensor_msgs::NavSatFix msg; + NavSatFixDataToMsg(data, msg); + navsatfix_gpgga_pub_.publish(msg); + } + }); } } } diff --git a/fixposition_driver_ros2/CATKIN_IGNORE b/fixposition_driver_ros2/CATKIN_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 609ca39..a01336d 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -72,6 +72,7 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr navsatfix_pub_; rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; + rclcpp::Publisher::SharedPtr navsatfix_gpgga_pub_; rclcpp::Publisher::SharedPtr odometry_pub_; rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 0046534..7a2edff 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, navsatfix_pub_(node_->create_publisher("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(node_->create_publisher("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(node_->create_publisher("/fixposition/gnss2", 100)), + navsatfix_gpgga_pub_(node_->create_publisher("/fixposition/gpgga", 100)), odometry_pub_(node_->create_publisher("/fixposition/odometry", 100)), poiimu_pub_(node_->create_publisher("/fixposition/poiimu", 100)), vrtk_pub_(node_->create_publisher("/fixposition/vrtk", 100)), @@ -183,6 +185,15 @@ void FixpositionDriverNode::RegisterObservers() { static_br_->sendTransform(tf); } }); + } else if (format == "GPGGA") { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) { + // GPGGA Observer Lambda + if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) { + sensor_msgs::NavSatFix msg; + NavSatFixDataToMsg(data, msg); + navsatfix_gpgga_pub_.publish(msg); + } + }); } } } From 57891eb50ee783ffaae709f3535d8198aeb10f4e Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 11 Jan 2024 16:06:20 +0100 Subject: [PATCH 06/15] Patched implementation --- .../fixposition_driver_lib/converter/gpgga.hpp | 3 +-- fixposition_driver_lib/src/fixposition_driver.cpp | 4 +--- fixposition_driver_lib/src/gpgga.cpp | 14 +++++++++++--- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp index 83fbb3a..5f13c33 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp @@ -58,8 +58,7 @@ class GpggaConverter : public BaseAsciiConverter { NavSatFixData msg_; std::vector obs_; const std::string header_ = "LLH"; - static constexpr const int kVersion_ = 1; - static constexpr const int kSize_ = 14; + static constexpr const int kSize_ = 15; }; } // namespace fixposition #endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index a53711c..8e20580 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -233,7 +233,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); // if it doesn't start with FP then do nothing - if ((tokens.at(0) != "FP") || (tokens.at(0) != "GPGGA")) { + if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA")) { return; } @@ -245,8 +245,6 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { _header = tokens.at(1); } const std::string header = _header; - std::cout << "Header: " << _header << std::endl; - std::cout << "Header: " << header << std::endl; // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU" if (a_converters_[header] != nullptr) { diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp index bfeb29a..eebfe99 100644 --- a/fixposition_driver_lib/src/gpgga.cpp +++ b/fixposition_driver_lib/src/gpgga.cpp @@ -46,12 +46,20 @@ void GpggaConverter::ConvertTokens(const std::vector& tokens) { } // Header stamps - msg_.stamp = ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx)); + msg_.stamp = times::GpsTime(0, 0); // ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx)); msg_.frame_id = "LLH"; // LLH coordinates - msg_.latitude = StringToDouble(tokens.at(lat_idx)); - msg_.longitude = StringToDouble(tokens.at(lon_idx)); + const std::string _latstr = tokens.at(lat_idx); + double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; + msg_.latitude = _lat; + + const std::string _lonstr = tokens.at(lon_idx); + double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; + msg_.longitude = _lon; + msg_.altitude = StringToDouble(tokens.at(alt_idx)); // Covariance diagonals From 5ff34dbdda3728e0dcc39dd6cdffc1a00a6309d2 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 12 Jan 2024 16:55:49 +0100 Subject: [PATCH 07/15] Added NMEA epoch and GPZDA handler --- fixposition_driver_lib/CMakeLists.txt | 1 + .../converter/gpgga.hpp | 4 +- .../converter/gpzda.hpp | 65 +++++++++++ .../fixposition_driver_lib/msg_data.hpp | 19 +++ .../src/fixposition_driver.cpp | 7 +- fixposition_driver_lib/src/gpgga.cpp | 5 +- fixposition_driver_lib/src/gpzda.cpp | 108 ++++++++++++++++++ .../fixposition_driver_node.hpp | 20 +++- fixposition_driver_ros1/launch/tcp.yaml | 2 +- .../src/fixposition_driver_node.cpp | 43 ++++++- 10 files changed, 260 insertions(+), 14 deletions(-) create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp create mode 100644 fixposition_driver_lib/src/gpzda.cpp diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 859d3c3..f1bf604 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -30,6 +30,7 @@ add_library( src/helper.cpp src/parser.cpp src/gpgga.cpp + src/gpzda.cpp ) target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp index 5f13c33..98d339b 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp @@ -28,7 +28,7 @@ namespace fixposition { class GpggaConverter : public BaseAsciiConverter { public: - using GpggaObserver = std::function; + using GpggaObserver = std::function; /** * @brief Construct a new GpggaConverter * @@ -55,7 +55,7 @@ class GpggaConverter : public BaseAsciiConverter { void AddObserver(GpggaObserver ob) { obs_.push_back(ob); } private: - NavSatFixData msg_; + GpggaData msg_; std::vector obs_; const std::string header_ = "LLH"; static constexpr const int kSize_ = 15; diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp new file mode 100644 index 0000000..a2447b2 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp @@ -0,0 +1,65 @@ +/** + * @file + * @brief Declaration of GpzdaConverter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include +#include +#include +#include + +namespace fixposition { + +class GpzdaConverter : public BaseAsciiConverter { + public: + using GpzdaObserver = std::function; + /** + * @brief Construct a new GpzdaConverter + * + */ + GpzdaConverter() {} + + ~GpzdaConverter() = default; + + /** + * @brief Take comma-delimited tokens of GPZDA message, convert to Data structs and if available, + * call observers + * Example: + * $GPZDA,090411.0001,10,10,2023,00,00*69\r\n + * + * @param[in] tokens message split in tokens + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(GpzdaObserver ob) { obs_.push_back(ob); } + + private: + GpzdaData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kSize_ = 7; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp index ecf9d57..8ce23f2 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp @@ -132,5 +132,24 @@ struct NavSatFixData { NavSatFixData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); } }; +struct GpggaData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string time; + double latitude; + double longitude; + double altitude; + Eigen::Matrix cov; + int position_covariance_type; + GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); } +}; + +struct GpzdaData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string time; + std::string date; + times::GpsTime stamp; + GpzdaData() : time(""), date("") {} +}; + } // namespace fixposition #endif //__FIXPOSITION_DRIVER_LIB_MSG_DATA__ diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 8e20580..4c0cc03 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -140,6 +141,8 @@ bool FixpositionDriver::InitializeConverters() { a_converters_["CORRIMU"] = std::unique_ptr(new ImuConverter(true)); } else if (format == "GPGGA") { a_converters_["GPGGA"] = std::unique_ptr(new GpggaConverter()); + } else if (format == "GPZDA") { + a_converters_["GPZDA"] = std::unique_ptr(new GpzdaConverter()); } else if (format == "TF") { if (a_converters_.find("TF") == a_converters_.end()) { a_converters_["TF"] = std::unique_ptr(new TfConverter()); @@ -233,7 +236,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); // if it doesn't start with FP then do nothing - if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA")) { + if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA")) { return; } @@ -241,6 +244,8 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { std::string _header; if (tokens.at(0) == "GPGGA") { _header = "GPGGA"; + } else if (tokens.at(0) == "GPZDA") { + _header = "GPZDA"; } else { _header = tokens.at(1); } diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp index eebfe99..52e129a 100644 --- a/fixposition_driver_lib/src/gpgga.cpp +++ b/fixposition_driver_lib/src/gpgga.cpp @@ -41,13 +41,12 @@ void GpggaConverter::ConvertTokens(const std::vector& tokens) { bool ok = tokens.size() == kSize_; if (!ok) { std::cout << "Error in parsing GPGGA string with " << tokens.size() << " fields! GPGGA message will be empty.\n"; - msg_ = NavSatFixData(); + msg_ = GpggaData(); return; } // Header stamps - msg_.stamp = times::GpsTime(0, 0); // ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx)); - msg_.frame_id = "LLH"; + msg_.time = tokens.at(time_idx); // LLH coordinates const std::string _latstr = tokens.at(lat_idx); diff --git a/fixposition_driver_lib/src/gpzda.cpp b/fixposition_driver_lib/src/gpzda.cpp new file mode 100644 index 0000000..e774de8 --- /dev/null +++ b/fixposition_driver_lib/src/gpzda.cpp @@ -0,0 +1,108 @@ +/** + * @file + * @brief Implementation of GpzdaConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int time_idx = 1; +static constexpr const int day_idx = 2; +static constexpr const int month_idx = 3; +static constexpr const int year_idx = 4; +static constexpr const int local_hr_idx = 5; +static constexpr const int local_min_idx = 6; + +#include +#include +#include + +// Function to convert UTC time with milliseconds to GPS time +void convertToGPSTime(const std::string& utcTimeString, std::string& gpsWeek, std::string& gpsTimeOfWeek) { + // Define constants + const double secondsInWeek = 604800.0; // 7 days in seconds + + // Parse the input string + std::tm tmTime = {}; + std::istringstream iss(utcTimeString); + iss >> std::get_time(&tmTime, "%d/%m/%Y %H:%M:%S"); + + // Read milliseconds from input + char dot; + std::string milliseconds; + iss >> dot >> milliseconds; + double ms = std::stod("0." + milliseconds); + + if (iss.fail()) { + std::cerr << "Error parsing input string.\n"; + return; + } + + // Convert UTC time to time since epoch + std::time_t utcTime = std::mktime(&tmTime); + + // GPS epoch time (January 6, 1980) + std::tm gpsEpoch = {}; + gpsEpoch.tm_year = 80; // years since 1900 + gpsEpoch.tm_mon = 0; // months since January + gpsEpoch.tm_mday = 6; // day of the month + std::time_t gpsEpochTime = std::mktime(&gpsEpoch); + + // Calculate GPS time of week and GPS week number + double timeDifference = std::difftime(utcTime, gpsEpochTime); + int gpsWeekNumber = static_cast(std::floor(timeDifference / secondsInWeek)); + double gpsTime = std::fmod(timeDifference, secondsInWeek); + + // Add milliseconds to GPS time + gpsTime += (18 + ms); + + // Convert results to strings + std::ostringstream ossWeek, ossTime; + ossWeek << gpsWeekNumber; + ossTime << std::fixed << std::setprecision(6) << gpsTime; + + gpsWeek = ossWeek.str(); + gpsTimeOfWeek = ossTime.str(); +} + +void GpzdaConverter::ConvertTokens(const std::vector& tokens) { + // Check if message size is wrong + bool ok = tokens.size() == kSize_; + if (!ok) { + std::cout << "Error in parsing GPZDA string with " << tokens.size() << " fields! GPZDA message will be empty.\n"; + msg_ = GpzdaData(); + return; + } + + // Populate time fields + msg_.time = tokens.at(time_idx); + msg_.date = tokens.at(day_idx) + '/' + tokens.at(month_idx) + '/' + tokens.at(year_idx); + + // Generate GPS timestamp + std::string utcTimeString = msg_.date + " " + msg_.time.substr(0,2) + ":" + msg_.time.substr(2,2) + ":" + msg_.time.substr(4); + std::string gps_tow, gps_week; + convertToGPSTime(utcTimeString, gps_week, gps_tow); + msg_.stamp = ConvertGpsTime(gps_week, gps_tow); + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index c58625e..461aa52 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -52,6 +52,20 @@ class FixpositionDriverNode : public FixpositionDriver { void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg); + struct NmeaMessage { + GpggaData gpgga; + GpzdaData gpzda; + NavSatFixData msg; + + bool checkEpoch() { + if (gpgga.time.compare(gpzda.time) == 0) { + return true; + } else { + return false; + } + } + }; + private: /** * @brief Observer Functions to publish NavSatFix from BestGnssPos @@ -61,6 +75,8 @@ class FixpositionDriverNode : public FixpositionDriver { */ void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + void PublishNmea(NmeaMessage data); + ros::NodeHandle nh_; ros::Subscriber ws_sub_; //!< wheelspeed message subscriber @@ -69,7 +85,7 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher navsatfix_pub_; ros::Publisher navsatfix_gnss1_pub_; ros::Publisher navsatfix_gnss2_pub_; - ros::Publisher navsatfix_gpgga_pub_; + ros::Publisher nmea_pub_; ros::Publisher odometry_pub_; //!< ECEF Odometry ros::Publisher poiimu_pub_; //!< Bias corrected IMU ros::Publisher vrtk_pub_; //!< VRTK message @@ -77,6 +93,8 @@ class FixpositionDriverNode : public FixpositionDriver { 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 + NmeaMessage nmea_message_; + tf2_ros::TransformBroadcaster br_; tf2_ros::StaticTransformBroadcaster static_br_; }; diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index 9d535e2..c10e573 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -1,5 +1,5 @@ fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "TF"] + formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "TF"] type: tcp port: "21000" ip: "10.0.2.1" # change to VRTK2's IP address in the network diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index adb646b..4878355 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -42,7 +43,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para navsatfix_pub_(nh_.advertise("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(nh_.advertise("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(nh_.advertise("/fixposition/gnss2", 100)), - navsatfix_gpgga_pub_(nh_.advertise("/fixposition/gpgga", 100)), + nmea_pub_(nh_.advertise("/fixposition/nmea", 100)), // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), @@ -50,9 +51,13 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para 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)) { + ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 100, &FixpositionDriverNode::WsCallback, this, ros::TransportHints().tcpNoDelay()); + + + Connect(); RegisterObservers(); } @@ -157,18 +162,44 @@ void FixpositionDriverNode::RegisterObservers() { } }); } else if (format == "GPGGA") { - dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) { // GPGGA Observer Lambda - if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) { - sensor_msgs::NavSatFix msg; - NavSatFixDataToMsg(data, msg); - navsatfix_gpgga_pub_.publish(msg); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gpgga = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPZDA") { + dynamic_cast(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) { + // GPZDA Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gpzda = data; + PublishNmea(nmea_message_); } }); } } } +void FixpositionDriverNode::PublishNmea(NmeaMessage data) { + // If epoch message is complete, generate NMEA output + if (data.checkEpoch()) { + sensor_msgs::NavSatFix msg; + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp)); + msg.header.frame_id = "LLH"; + msg.latitude = data.gpgga.latitude; + msg.longitude = data.gpgga.longitude; + msg.altitude = data.gpgga.altitude; + + Eigen::Map> cov_map = + Eigen::Map>(msg.position_covariance.data()); + cov_map = data.gpgga.cov; + + msg.position_covariance_type = data.gpgga.position_covariance_type; + nmea_pub_.publish(msg); + } +} + void FixpositionDriverNode::Run() { ros::Rate rate(params_.fp_output.rate); while (ros::ok()) { From a9bf143c7d2c1c631e10df8cc0c06dad8cc7c71f Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 12 Jan 2024 18:00:09 +0100 Subject: [PATCH 08/15] Added new NMEA.msg and integrated GPRMC to nmea topic --- fixposition_driver_lib/CMakeLists.txt | 1 + .../converter/gprmc.hpp | 64 ++++++++++++++++ .../fixposition_driver_lib/msg_data.hpp | 11 +++ .../src/fixposition_driver.cpp | 7 +- fixposition_driver_lib/src/gprmc.cpp | 73 +++++++++++++++++++ fixposition_driver_ros1/CMakeLists.txt | 1 + .../fixposition_driver_ros1/data_to_ros1.hpp | 1 + .../fixposition_driver_node.hpp | 4 +- fixposition_driver_ros1/launch/tcp.yaml | 2 +- fixposition_driver_ros1/msg/NMEA.msg | 66 +++++++++++++++++ .../src/fixposition_driver_node.cpp | 36 ++++++++- fixposition_driver_ros2/CMakeLists.txt | 1 + 12 files changed, 260 insertions(+), 7 deletions(-) create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp create mode 100644 fixposition_driver_lib/src/gprmc.cpp create mode 100644 fixposition_driver_ros1/msg/NMEA.msg diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index f1bf604..b1b35af 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -31,6 +31,7 @@ add_library( src/parser.cpp src/gpgga.cpp src/gpzda.cpp + src/gprmc.cpp ) target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp new file mode 100644 index 0000000..832de05 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp @@ -0,0 +1,64 @@ +/** + * @file + * @brief Declaration of GprmcConverter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ +#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ + +/* SYSTEM / STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include +#include +#include + +namespace fixposition { + +class GprmcConverter : public BaseAsciiConverter { + public: + using GprmcObserver = std::function; + /** + * @brief Construct a new GprmcConverter + * + */ + GprmcConverter() {} + + ~GprmcConverter() = default; + + /** + * @brief Take comma-delimited tokens of GPRMC message, convert to Data structs and if available, + * call observers + * Example: + * $GPRMC,151227.40,A,4723.54036,N,00826.88672,E,0.0,81.6,111022,,,R*7C\r\n + * + * @param[in] tokens message split in tokens + */ + void ConvertTokens(const std::vector& tokens) final; + + /** + * @brief Add Observer to call at the end of ConvertTokens() + * + * @param[in] ob + */ + void AddObserver(GprmcObserver ob) { obs_.push_back(ob); } + + private: + GprmcData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kSize_ = 13; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp index 8ce23f2..ff38989 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp @@ -151,5 +151,16 @@ struct GpzdaData { GpzdaData() : time(""), date("") {} }; +struct GprmcData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string time; + std::string mode; + double latitude; + double longitude; + double speed; + double course; + GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0) {} +}; + } // namespace fixposition #endif //__FIXPOSITION_DRIVER_LIB_MSG_DATA__ diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 4c0cc03..1f2fb78 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -143,6 +144,8 @@ bool FixpositionDriver::InitializeConverters() { a_converters_["GPGGA"] = std::unique_ptr(new GpggaConverter()); } else if (format == "GPZDA") { a_converters_["GPZDA"] = std::unique_ptr(new GpzdaConverter()); + } else if (format == "GPRMC") { + a_converters_["GPRMC"] = std::unique_ptr(new GprmcConverter()); } else if (format == "TF") { if (a_converters_.find("TF") == a_converters_.end()) { a_converters_["TF"] = std::unique_ptr(new TfConverter()); @@ -236,7 +239,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); // if it doesn't start with FP then do nothing - if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA")) { + if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPRMC")) { return; } @@ -246,6 +249,8 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { _header = "GPGGA"; } else if (tokens.at(0) == "GPZDA") { _header = "GPZDA"; + } else if (tokens.at(0) == "GPRMC") { + _header = "GPRMC"; } else { _header = tokens.at(1); } diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp new file mode 100644 index 0000000..2a0d5a0 --- /dev/null +++ b/fixposition_driver_lib/src/gprmc.cpp @@ -0,0 +1,73 @@ +/** + * @file + * @brief Implementation of GprmcConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +/// msg field indices +static constexpr const int time_idx = 1; +static constexpr const int status_idx = 2; +static constexpr const int lat_idx = 3; +static constexpr const int lat_ns_idx = 4; +static constexpr const int lon_idx = 5; +static constexpr const int lon_ew_idx = 6; +static constexpr const int speed_idx = 7; +static constexpr const int course_idx = 8; +static constexpr const int date_idx = 9; +static constexpr const int magvar_idx = 10; +static constexpr const int magvar_ew = 11; +static constexpr const int mode_idx = 12; + +void GprmcConverter::ConvertTokens(const std::vector& tokens) { + // Check if message size is wrong + bool ok = tokens.size() == kSize_; + if (!ok) { + std::cout << "Error in parsing GPRMC string with " << tokens.size() << " fields! GPRMC message will be empty.\n"; + msg_ = GprmcData(); + return; + } + + // Header stamps + msg_.time = tokens.at(time_idx); + + // LLH coordinates + const std::string _latstr = tokens.at(lat_idx); + double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; + if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; + msg_.latitude = _lat; + + const std::string _lonstr = tokens.at(lon_idx); + double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; + if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; + msg_.longitude = _lon; + + // Speed and course over groung + msg_.speed = StringToDouble(tokens.at(speed_idx)) * 1852.0 / 3600.0; + msg_.course = StringToDouble(tokens.at(course_idx)); + + // Get GPS status + msg_.mode = tokens.at(mode_idx); + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 07e1898..74ce039 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -26,6 +26,7 @@ add_message_files( FILES VRTK.msg Speed.msg + NMEA.msg ) generate_messages( diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 0bb259f..2f5c093 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -27,6 +27,7 @@ /* PACKAGE */ #include +#include namespace fixposition { /** diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 461aa52..bd87663 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -55,10 +55,10 @@ class FixpositionDriverNode : public FixpositionDriver { struct NmeaMessage { GpggaData gpgga; GpzdaData gpzda; - NavSatFixData msg; + GprmcData gprmc; bool checkEpoch() { - if (gpgga.time.compare(gpzda.time) == 0) { + if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) { return true; } else { return false; diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index c10e573..7bf49c4 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -1,5 +1,5 @@ fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "TF"] + formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: tcp port: "21000" ip: "10.0.2.1" # change to VRTK2's IP address in the network diff --git a/fixposition_driver_ros1/msg/NMEA.msg b/fixposition_driver_ros1/msg/NMEA.msg new file mode 100644 index 0000000..98d2649 --- /dev/null +++ b/fixposition_driver_ros1/msg/NMEA.msg @@ -0,0 +1,66 @@ +#################################################################################################### +# +# Copyright (c) 2023 ___ ___ +# \\ \\ / / +# \\ \\/ / +# / /\\ \\ +# /__/ \\__\\ Fixposition AG +# +#################################################################################################### +# +# Fixposition NMEA Message +# +# +#################################################################################################### + +# Navigation Satellite fix for any Global Navigation Satellite System +# +# Specified using the WGS 84 reference ellipsoid + +# header.stamp specifies the ROS time for this measurement (the +# corresponding satellite time may be reported using the +# sensor_msgs/TimeReference message). +# +# header.frame_id is the frame of reference reported by the satellite +# receiver, usually the location of the antenna. This is a +# Euclidean frame relative to the vehicle, not a reference +# ellipsoid. +Header header + +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude + +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude + +# Altitude [m]. Positive is above the WGS 84 ellipsoid. +float64 altitude + +# Speed over ground [m/s] +float64 speed + +# Course over ground [deg] +float64 course + +# Position covariance [m^2] defined relative to a tangential plane +# through the reported position. The components are East, North, and +# Up (ENU), in row-major order. +# +# Beware: this coordinate system exhibits singularities at the poles. + +float64[9] position_covariance + +# If the covariance of the fix is known, fill it in completely. If the +# GPS receiver provides the variance of each measurement, put them +# along the diagonal. If only Dilution of Precision is available, +# estimate an approximate covariance from that. + +uint8 COVARIANCE_TYPE_UNKNOWN = 0 +uint8 COVARIANCE_TYPE_APPROXIMATED = 1 +uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 +uint8 COVARIANCE_TYPE_KNOWN = 3 + +uint8 position_covariance_type + +# Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N +string mode \ No newline at end of file diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 4878355..161426c 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -43,7 +44,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para navsatfix_pub_(nh_.advertise("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(nh_.advertise("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(nh_.advertise("/fixposition/gnss2", 100)), - nmea_pub_(nh_.advertise("/fixposition/nmea", 100)), + nmea_pub_(nh_.advertise("/fixposition/nmea", 100)), // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), @@ -177,6 +178,14 @@ void FixpositionDriverNode::RegisterObservers() { PublishNmea(nmea_message_); } }); + } else if (format == "GPRMC") { + dynamic_cast(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) { + // GPRMC Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gprmc = data; + PublishNmea(nmea_message_); + } + }); } } } @@ -184,18 +193,39 @@ void FixpositionDriverNode::RegisterObservers() { void FixpositionDriverNode::PublishNmea(NmeaMessage data) { // If epoch message is complete, generate NMEA output if (data.checkEpoch()) { - sensor_msgs::NavSatFix msg; + // Generate new message + fixposition_driver_ros1::NMEA msg; + + // ROS Header msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp)); msg.header.frame_id = "LLH"; + + // Latitude [degrees]. Positive is north of equator; negative is south msg.latitude = data.gpgga.latitude; + + // Longitude [degrees]. Positive is east of prime meridian; negative is west msg.longitude = data.gpgga.longitude; + + // Altitude [m]. Positive is above the WGS 84 ellipsoid msg.altitude = data.gpgga.altitude; + // Speed over ground [m/s] + msg.speed = data.gprmc.speed; + + // Course over ground [deg] + msg.course = data.gprmc.course; + + // TODO: Get better position covariance from NMEA-GP-GST + // Position covariance [m^2] Eigen::Map> cov_map = Eigen::Map>(msg.position_covariance.data()); cov_map = data.gpgga.cov; - msg.position_covariance_type = data.gpgga.position_covariance_type; + + // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N + msg.mode = data.gprmc.mode; + + // Publish message nmea_pub_.publish(msg); } } diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index ca2b66f..7353ac4 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(fixposition_driver_lib REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/VRTK.msg msg/Speed.msg + msg/NMEA.msg DEPENDENCIES std_msgs nav_msgs From b243cdeb5b819e15a45a30438a05d901fa12c3bc Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 12 Jan 2024 18:11:25 +0100 Subject: [PATCH 09/15] ROS2 implementation - Please test --- .../fixposition_driver_ros2/data_to_ros2.hpp | 1 + .../fixposition_driver_node.hpp | 18 ++++- fixposition_driver_ros2/msg/NMEA.msg | 66 ++++++++++++++++++ .../src/fixposition_driver_node.cpp | 69 +++++++++++++++++-- 4 files changed, 147 insertions(+), 7 deletions(-) create mode 100644 fixposition_driver_ros2/msg/NMEA.msg diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index 0bcdc16..8afe911 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -37,6 +37,7 @@ /* PACKAGE */ #include +#include namespace fixposition { diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index a01336d..180697b 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -55,6 +55,20 @@ class FixpositionDriverNode : public FixpositionDriver { void WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg); + struct NmeaMessage { + GpggaData gpgga; + GpzdaData gpzda; + GprmcData gprmc; + + bool checkEpoch() { + if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) { + return true; + } else { + return false; + } + } + }; + private: /** * @brief Observer Functions to publish NavSatFix from BestGnssPos @@ -72,7 +86,7 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr navsatfix_pub_; rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; - rclcpp::Publisher::SharedPtr navsatfix_gpgga_pub_; + rclcpp::Publisher::SharedPtr nmea_pub_; rclcpp::Publisher::SharedPtr odometry_pub_; rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message @@ -84,6 +98,8 @@ class FixpositionDriverNode : public FixpositionDriver { std::shared_ptr br_; std::shared_ptr static_br_; + + NmeaMessage nmea_message_; }; } // namespace fixposition diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg new file mode 100644 index 0000000..98d2649 --- /dev/null +++ b/fixposition_driver_ros2/msg/NMEA.msg @@ -0,0 +1,66 @@ +#################################################################################################### +# +# Copyright (c) 2023 ___ ___ +# \\ \\ / / +# \\ \\/ / +# / /\\ \\ +# /__/ \\__\\ Fixposition AG +# +#################################################################################################### +# +# Fixposition NMEA Message +# +# +#################################################################################################### + +# Navigation Satellite fix for any Global Navigation Satellite System +# +# Specified using the WGS 84 reference ellipsoid + +# header.stamp specifies the ROS time for this measurement (the +# corresponding satellite time may be reported using the +# sensor_msgs/TimeReference message). +# +# header.frame_id is the frame of reference reported by the satellite +# receiver, usually the location of the antenna. This is a +# Euclidean frame relative to the vehicle, not a reference +# ellipsoid. +Header header + +# Latitude [degrees]. Positive is north of equator; negative is south. +float64 latitude + +# Longitude [degrees]. Positive is east of prime meridian; negative is west. +float64 longitude + +# Altitude [m]. Positive is above the WGS 84 ellipsoid. +float64 altitude + +# Speed over ground [m/s] +float64 speed + +# Course over ground [deg] +float64 course + +# Position covariance [m^2] defined relative to a tangential plane +# through the reported position. The components are East, North, and +# Up (ENU), in row-major order. +# +# Beware: this coordinate system exhibits singularities at the poles. + +float64[9] position_covariance + +# If the covariance of the fix is known, fill it in completely. If the +# GPS receiver provides the variance of each measurement, put them +# along the diagonal. If only Dilution of Precision is available, +# estimate an approximate covariance from that. + +uint8 COVARIANCE_TYPE_UNKNOWN = 0 +uint8 COVARIANCE_TYPE_APPROXIMATED = 1 +uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 +uint8 COVARIANCE_TYPE_KNOWN = 3 + +uint8 position_covariance_type + +# Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N +string mode \ No newline at end of file diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 7a2edff..f9b1466 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include #include @@ -44,7 +46,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, navsatfix_pub_(node_->create_publisher("/fixposition/navsatfix", 100)), navsatfix_gnss1_pub_(node_->create_publisher("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(node_->create_publisher("/fixposition/gnss2", 100)), - navsatfix_gpgga_pub_(node_->create_publisher("/fixposition/gpgga", 100)), + nmea_pub_(node_->create_publisher("/fixposition/nmea", 100)), odometry_pub_(node_->create_publisher("/fixposition/odometry", 100)), poiimu_pub_(node_->create_publisher("/fixposition/poiimu", 100)), vrtk_pub_(node_->create_publisher("/fixposition/vrtk", 100)), @@ -186,18 +188,73 @@ void FixpositionDriverNode::RegisterObservers() { } }); } else if (format == "GPGGA") { - dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) { // GPGGA Observer Lambda - if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) { - sensor_msgs::NavSatFix msg; - NavSatFixDataToMsg(data, msg); - navsatfix_gpgga_pub_.publish(msg); + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gpgga = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPZDA") { + dynamic_cast(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) { + // GPZDA Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gpzda = data; + PublishNmea(nmea_message_); + } + }); + } else if (format == "GPRMC") { + dynamic_cast(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) { + // GPRMC Observer Lambda + if (nmea_pub_.getNumSubscribers() > 0) { + nmea_message_.gprmc = data; + PublishNmea(nmea_message_); } }); } } } +void FixpositionDriverNode::PublishNmea(NmeaMessage data) { + // If epoch message is complete, generate NMEA output + if (data.checkEpoch()) { + // Generate new message + fixposition_driver_ros1::NMEA msg; + + // ROS Header + msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp)); + msg.header.frame_id = "LLH"; + + // Latitude [degrees]. Positive is north of equator; negative is south + msg.latitude = data.gpgga.latitude; + + // Longitude [degrees]. Positive is east of prime meridian; negative is west + msg.longitude = data.gpgga.longitude; + + // Altitude [m]. Positive is above the WGS 84 ellipsoid + msg.altitude = data.gpgga.altitude; + + // Speed over ground [m/s] + msg.speed = data.gprmc.speed; + + // Course over ground [deg] + msg.course = data.gprmc.course; + + // TODO: Get better position covariance from NMEA-GP-GST + // Position covariance [m^2] + Eigen::Map> cov_map = + Eigen::Map>(msg.position_covariance.data()); + cov_map = data.gpgga.cov; + msg.position_covariance_type = data.gpgga.position_covariance_type; + + // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N + msg.mode = data.gprmc.mode; + + // Publish message + nmea_pub_.publish(msg); + } +} + void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) { FixpositionDriver::WsCallback(msg->speeds); } From 4ece5416f095b8180cecc495cb2a607ce7e62976 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 15 Jan 2024 16:08:25 +0100 Subject: [PATCH 10/15] Added unit conversion as constant --- fixposition_driver_lib/src/gprmc.cpp | 7 +++++-- fixposition_driver_ros1/launch/tcp.yaml | 2 +- fixposition_driver_ros2/CATKIN_IGNORE | 0 3 files changed, 6 insertions(+), 3 deletions(-) delete mode 100644 fixposition_driver_ros2/CATKIN_IGNORE diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp index 2a0d5a0..e09fc23 100644 --- a/fixposition_driver_lib/src/gprmc.cpp +++ b/fixposition_driver_lib/src/gprmc.cpp @@ -20,6 +20,9 @@ namespace fixposition { +// unit transformation constant +static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s + /// msg field indices static constexpr const int time_idx = 1; static constexpr const int status_idx = 2; @@ -57,8 +60,8 @@ void GprmcConverter::ConvertTokens(const std::vector& tokens) { if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; msg_.longitude = _lon; - // Speed and course over groung - msg_.speed = StringToDouble(tokens.at(speed_idx)) * 1852.0 / 3600.0; + // Speed [m/s] and course [deg] over ground + msg_.speed = StringToDouble(tokens.at(speed_idx)) * knots_to_ms; msg_.course = StringToDouble(tokens.at(course_idx)); // Get GPS status diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index 7bf49c4..44ecfc4 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -2,7 +2,7 @@ fp_output: formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: tcp port: "21000" - ip: "10.0.2.1" # change to VRTK2's IP address in the network + ip: "127.0.0.1" # change to VRTK2's IP address in the network rate: 200 reconnect_delay: 5.0 # wait time in [s] until retry connection diff --git a/fixposition_driver_ros2/CATKIN_IGNORE b/fixposition_driver_ros2/CATKIN_IGNORE deleted file mode 100644 index e69de29..0000000 From 6dd89eb6fdfc8939ed8628caa4d5961309797951 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 15 Jan 2024 16:49:40 +0100 Subject: [PATCH 11/15] ROS2 patch fix --- fixposition_driver_lib/src/gprmc.cpp | 2 +- fixposition_driver_ros1/launch/tcp.yaml | 2 +- .../fixposition_driver_node.hpp | 2 ++ fixposition_driver_ros2/launch/tcp.yaml | 2 +- fixposition_driver_ros2/msg/NMEA.msg | 9 +++------ .../src/fixposition_driver_node.cpp | 12 ++++++------ 6 files changed, 14 insertions(+), 15 deletions(-) diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp index e09fc23..4d6a6f4 100644 --- a/fixposition_driver_lib/src/gprmc.cpp +++ b/fixposition_driver_lib/src/gprmc.cpp @@ -20,7 +20,7 @@ namespace fixposition { -// unit transformation constant +// unit conversion constant static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s /// msg field indices diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index 44ecfc4..7bf49c4 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -2,7 +2,7 @@ fp_output: formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: tcp port: "21000" - ip: "127.0.0.1" # change to VRTK2's IP address in the network + ip: "10.0.2.1" # change to VRTK2's IP address in the network rate: 200 reconnect_delay: 5.0 # wait time in [s] until retry connection diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 180697b..0a53850 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -77,6 +77,8 @@ class FixpositionDriverNode : public FixpositionDriver { * @param[in] payload */ void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + + void PublishNmea(NmeaMessage data); std::shared_ptr node_; rclcpp::Subscription::SharedPtr ws_sub_; //!< wheelspeed message subscriber diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml index 9348466..09d7079 100644 --- a/fixposition_driver_ros2/launch/tcp.yaml +++ b/fixposition_driver_ros2/launch/tcp.yaml @@ -1,7 +1,7 @@ fixposition_driver_ros2: ros__parameters: fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"] + formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: "tcp" port: "21000" ip: "10.0.2.1" # change to VRTK2's IP address in the network diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg index 98d2649..0c6064e 100644 --- a/fixposition_driver_ros2/msg/NMEA.msg +++ b/fixposition_driver_ros2/msg/NMEA.msg @@ -1,10 +1,7 @@ #################################################################################################### # -# Copyright (c) 2023 ___ ___ -# \\ \\ / / -# \\ \\/ / -# / /\\ \\ -# /__/ \\__\\ Fixposition AG +# Copyright (c) 2023 +# Fixposition AG # #################################################################################################### # @@ -25,7 +22,7 @@ # receiver, usually the location of the antenna. This is a # Euclidean frame relative to the vehicle, not a reference # ellipsoid. -Header header +std_msgs/Header header # Latitude [degrees]. Positive is north of equator; negative is south. float64 latitude diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index f9b1466..7c1d2b9 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -190,7 +190,7 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "GPGGA") { dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) { // GPGGA Observer Lambda - if (nmea_pub_.getNumSubscribers() > 0) { + if (nmea_pub_->get_subscription_count() > 0) { nmea_message_.gpgga = data; PublishNmea(nmea_message_); } @@ -198,7 +198,7 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "GPZDA") { dynamic_cast(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) { // GPZDA Observer Lambda - if (nmea_pub_.getNumSubscribers() > 0) { + if (nmea_pub_->get_subscription_count() > 0) { nmea_message_.gpzda = data; PublishNmea(nmea_message_); } @@ -206,7 +206,7 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "GPRMC") { dynamic_cast(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) { // GPRMC Observer Lambda - if (nmea_pub_.getNumSubscribers() > 0) { + if (nmea_pub_->get_subscription_count() > 0) { nmea_message_.gprmc = data; PublishNmea(nmea_message_); } @@ -219,10 +219,10 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) { // If epoch message is complete, generate NMEA output if (data.checkEpoch()) { // Generate new message - fixposition_driver_ros1::NMEA msg; + fixposition_driver_ros2::msg::NMEA msg; // ROS Header - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp)); + msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp); msg.header.frame_id = "LLH"; // Latitude [degrees]. Positive is north of equator; negative is south @@ -251,7 +251,7 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) { msg.mode = data.gprmc.mode; // Publish message - nmea_pub_.publish(msg); + nmea_pub_->publish(msg); } } From df585046bc5527849e043a95da2c1fff30f657f3 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 15 Jan 2024 17:02:55 +0100 Subject: [PATCH 12/15] Updated README with NMEA output --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 070bb80..b3ba34e 100644 --- a/README.md +++ b/README.md @@ -176,6 +176,12 @@ The output is published on the following: | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | +- The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., only GNSS, not Fusion) and heading estimate based on SOG and COG (i.e., the platform must be moving for it to be accurate) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** + + | Topic | Message Type | Frequency | Description | + | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | + | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height | + #### Vision-RTK2 IMU data - From RAWIMU, at 200Hz From b6747f764638cb0537bb49d67653a28357d1bc95 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 15 Jan 2024 17:14:37 +0100 Subject: [PATCH 13/15] Updated documentation --- README.md | 2 +- .../fixposition_driver_ros1/fixposition_driver_node.hpp | 8 ++++++++ .../fixposition_driver_ros2/fixposition_driver_node.hpp | 8 ++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index b3ba34e..34c7daf 100644 --- a/README.md +++ b/README.md @@ -176,7 +176,7 @@ The output is published on the following: | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | -- The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., only GNSS, not Fusion) and heading estimate based on SOG and COG (i.e., the platform must be moving for it to be accurate) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** +- The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (SOG) and course over ground (COG) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** | Topic | Message Type | Frequency | Description | | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index bd87663..13dc590 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -57,6 +57,9 @@ class FixpositionDriverNode : public FixpositionDriver { GpzdaData gpzda; GprmcData gprmc; + /** + * @brief Construct a new Fixposition Driver Node object + */ bool checkEpoch() { if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) { return true; @@ -75,6 +78,11 @@ class FixpositionDriverNode : public FixpositionDriver { */ void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + /** + * @brief Observer Function to publish NMEA message from GPGGA, GPRMC, and GPZDA once the GNSS epoch transmission is complete + * + * @param[in] data + */ void PublishNmea(NmeaMessage data); ros::NodeHandle nh_; diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 0a53850..6573259 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -60,6 +60,9 @@ class FixpositionDriverNode : public FixpositionDriver { GpzdaData gpzda; GprmcData gprmc; + /** + * @brief Check if GNSS epoch is complete + */ bool checkEpoch() { if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) { return true; @@ -78,6 +81,11 @@ class FixpositionDriverNode : public FixpositionDriver { */ void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload); + /** + * @brief Observer Function to publish NMEA message from GPGGA, GPRMC, and GPZDA once the GNSS epoch transmission is complete + * + * @param[in] data + */ void PublishNmea(NmeaMessage data); std::shared_ptr node_; From 77f9e0f5d86776e13645136b202640fcfe6d6d1c Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Tue, 23 Jan 2024 10:17:12 +0100 Subject: [PATCH 14/15] Updated ROS topic documentation --- README.md | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 34c7daf..f9dce85 100644 --- a/README.md +++ b/README.md @@ -146,7 +146,7 @@ The output is published on the following: #### Vision-RTK2 Fusion -- From ODOMETRY, at the configured frequency +- From **FP_A-ODOMETRY**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - Messages @@ -154,12 +154,11 @@ The output is published on the following: | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | | `/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/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. | -- From LLH, at the configured frequency +- From **FP_A-LLH**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): | Topic | Message Type | Frequency | Description | | ------------------------ | ----------------------- | ------------------------------ | ------------------------------ | @@ -169,14 +168,14 @@ The output is published on the following: **If GNSS Antenna positions are needed, please enable this on the sensor's configuration interface.** -- From NOV_B-BESTGNSSPOS, at the configured frequency, GNSS1 and GNSS2 raw antenna positions. +- From **NOV_B-BESTGNSSPOS_GNSS[1,2]**, at the configured frequency, GNSS1 and GNSS2 raw antenna positions (default 5Hz): | Topic | Message Type | Frequency | Description | | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | -- The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (SOG) and course over ground (COG) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** +- From **NMEA-GP-GGA_GNSS**, **NMEA-GP-RMC_GNSS**, and **NMEA-GP-ZDA_GNSS**, at the configured frequency (default 5Hz): The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (**SOG**) and course over ground (**COG**) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** | Topic | Message Type | Frequency | Description | | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | @@ -184,18 +183,24 @@ The output is published on the following: #### Vision-RTK2 IMU data -- From RAWIMU, at 200Hz +- From **FP_A-RAWIMU**, at 200Hz: | Topic | Message Type | Frequency | Description | | --------------------- | ----------------- | --------- | ----------------------------------------------------------------------------------------- | | `/fixposition/rawimu` | `sensor_msgs/Imu` | 200Hz | Raw (without bias correction) IMU acceleration and angular velocity data in FP_VRTK frame | -- From CORRIMU, at 200Hz +- From **FP_A-CORRIMU**, at 200Hz: | Topic | Message Type | Frequency | Description | | ---------------------- | ----------------- | --------- | -------------------------------------------------------------------------- | | `/fixposition/corrimu` | `sensor_msgs/Imu` | 200Hz | Bias Corrected IMU acceleration and angular velocity data in FP_VRTK frame | +- From **FP_A-TF_POIIMUH**, at 200Hz: + + | Topic | Message Type | Frequency | Description | + | ------------------------ | ----------------------- | ------------------------------ | ------------------------------ | + | `/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. | + #### Transforms - TFs: From aae8ce840d6481c264d2b03e615af63e7df0ded8 Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Tue, 23 Jan 2024 08:43:17 -0800 Subject: [PATCH 15/15] Mark ROS 1 dependencies correctly in package.xml (#43) * Tag out ros1 dependencies Signed-off-by: Emerson Knapp * Add missing dependencies and do some alphasorting Signed-off-by: Emerson Knapp --------- Signed-off-by: Emerson Knapp --- fixposition_driver_lib/package.xml | 9 +++++---- fixposition_driver_ros2/package.xml | 17 ++++++++++++----- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/fixposition_driver_lib/package.xml b/fixposition_driver_lib/package.xml index 42a797c..62ec541 100644 --- a/fixposition_driver_lib/package.xml +++ b/fixposition_driver_lib/package.xml @@ -11,14 +11,15 @@ catkin ament_cmake ament_cmake_gtest - roscpp - tf + boost + roscpp + tf nav_msgs sensor_msgs std_msgs geometry_msgs - message_generation - message_runtime + message_generation + message_runtime fixposition_gnss_tf cmake diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml index 97baa31..620e9e6 100644 --- a/fixposition_driver_ros2/package.xml +++ b/fixposition_driver_ros2/package.xml @@ -9,17 +9,24 @@ MIT ament_cmake - rclcpp rosidl_default_generators - rosidl_default_runtime + rosidl_interface_packages + builtin_interfaces + + fixposition_driver_lib + fixposition_gnss_tf + geometry_msgs nav_msgs sensor_msgs std_msgs - geometry_msgs - fixposition_gnss_tf - fixposition_driver_lib + tf2 + tf2_eigen + tf2_ros + + rclcpp + rosidl_default_runtime ament_cmake