diff --git a/README.md b/README.md index 75f0be3..f9dce85 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 @@ -136,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 @@ -144,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 | | ------------------------ | ----------------------- | ------------------------------ | ------------------------------ | @@ -159,38 +168,50 @@ 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 | +- 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 | + | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | + | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height | + #### 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: - | 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 +227,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 +249,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 +355,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 +458,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 +487,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 +513,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 +542,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 diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 6df5040..b1b35af 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -29,6 +29,9 @@ add_library( src/tf.cpp src/helper.cpp 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/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp new file mode 100644 index 0000000..98d339b --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp @@ -0,0 +1,64 @@ +/** + * @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: + GpggaData msg_; + std::vector obs_; + const std::string header_ = "LLH"; + static constexpr const int kSize_ = 15; +}; +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ 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/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..ff38989 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,35 @@ 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("") {} +}; + +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/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_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 3901539..1f2fb78 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -22,6 +22,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -137,6 +140,12 @@ 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 == "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()); @@ -230,12 +239,22 @@ 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") && (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPRMC")) { 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 if (tokens.at(0) == "GPZDA") { + _header = "GPZDA"; + } else if (tokens.at(0) == "GPRMC") { + _header = "GPRMC"; + } else { + _header = tokens.at(1); + } + const std::string header = _header; // 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..52e129a --- /dev/null +++ b/fixposition_driver_lib/src/gpgga.cpp @@ -0,0 +1,82 @@ +/** + * @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_ = GpggaData(); + 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; + + 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_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp new file mode 100644 index 0000000..4d6a6f4 --- /dev/null +++ b/fixposition_driver_lib/src/gprmc.cpp @@ -0,0 +1,76 @@ +/** + * @file + * @brief Implementation of GprmcConverter converter + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* SYSTEM / STL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +// unit conversion 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; +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 [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 + msg_.mode = tokens.at(mode_idx); + + // Process all observers + for (auto& ob : obs_) { + ob(msg_); + } +} + +} // namespace fixposition 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/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 5ebd542..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 @@ -16,6 +16,7 @@ #define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__ /* ROS */ #include +#include #include #include #include @@ -26,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 064ea13..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 @@ -52,6 +52,23 @@ class FixpositionDriverNode : public FixpositionDriver { void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg); + struct NmeaMessage { + GpggaData gpgga; + 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; + } else { + return false; + } + } + }; + private: /** * @brief Observer Functions to publish NavSatFix from BestGnssPos @@ -61,6 +78,13 @@ 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_; ros::Subscriber ws_sub_; //!< wheelspeed message subscriber @@ -69,6 +93,7 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher navsatfix_pub_; ros::Publisher navsatfix_gnss1_pub_; ros::Publisher navsatfix_gnss2_pub_; + ros::Publisher nmea_pub_; ros::Publisher odometry_pub_; //!< ECEF Odometry ros::Publisher poiimu_pub_; //!< Bias corrected IMU ros::Publisher vrtk_pub_; //!< VRTK message @@ -76,6 +101,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 f9165b6..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", "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 f834991..161426c 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -22,6 +22,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -41,16 +44,21 @@ 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)), // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), 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()); + + + Connect(); RegisterObservers(); } @@ -85,8 +93,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,18 +152,84 @@ 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 { static_br_.sendTransform(tf); } }); + } else if (format == "GPGGA") { + dynamic_cast(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) { + // GPGGA Observer Lambda + 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::Run() { ros::Rate rate(params_.fp_output.rate); while (ros::ok()) { 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.