update fixposition SDK, update FP_A-{RAW,CORR}IMU output (#85)

- Bump Fixposition SDK
- Add support FP_A-{RAW,CORR}IMU version 2
   - Wrap sensor_msgs/Imu into a custom message, which includes the new flags along the standard IMU data
   - That is, /fpa/{raw,corr}imu topics now contain two new fields "bias_comp" and "imu_status" and the imu data itself (type sensor_msgs/Imu) is in the field "data"
      - For backwards compatibility, users can subscribe to /fpa/{raw,corr}imu/data and ignore the new fields
      - However, since v2.102.119 of the VRTK2 software, the FP_A-CORRIMU is now always output and users _must_ consider and handle the new fields
   - See fixposition_driver_msgs/msg/FpaImu.msg for details

Author: flipflip8952

fixposition-sdk

@@ -0,0 +1,12 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-RAWIMU or FP_A-CORRIMU data
+
+bool bias_comp                               # Signal is bias compensated (true) or not (false), always false for RAWIMU, may be true for CORRIMU
+int8 imu_status                              # IMU bias status, see consts.IMU_STATUS_...
+sensor_msgs/Imu data                         # IMU data
+
+# Note that bias_comp and imu_status are available since version 2.119.0 of the Vision-RTK 2 software.
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A

fixposition_driver_msgs/msg/FpaImu.msg

@@ -12,7 +12,7 @@ int8 fusion_corr                             # Fusion measurement status: GNSS c
 int8 fusion_cam1                             # Fusion measurement status: camera, see consts.MEAS_STATUS_...
 int8 fusion_ws                               # Fusion measurement status: wheelspeed, see consts.MEAS_STATUS_...
 int8 fusion_markers                          # Fusion measurement status: markers, see consts.MEAS_STATUS_...
-int8 imu_status                              # IMU bias status, see IMU_STATUS_...
+int8 imu_status                              # IMU bias status, see consts.IMU_STATUS_...
 int8 imu_noise                               # IMU variance status, see consts.IMU_NOISE_...
 int8 imu_conv                                # IMU convergence status, see consts.IMU_CONV_...
 int8 gnss1_status                            # GNSS 1 fix status, see consts.GNSS_STATUS_...

fixposition_driver_msgs/msg/FpaOdomstatus.msg

@@ -35,6 +35,7 @@
 #include <fixposition_driver_msgs/FpaEoe.h>
 #include <fixposition_driver_msgs/FpaGnssant.h>
 #include <fixposition_driver_msgs/FpaGnsscorr.h>
+#include <fixposition_driver_msgs/FpaImu.h>
 #include <fixposition_driver_msgs/FpaImubias.h>
 #include <fixposition_driver_msgs/FpaLlh.h>
 #include <fixposition_driver_msgs/FpaOdomenu.h>

fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp

@@ -397,16 +397,20 @@ static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::Im
 
 void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        sensor_msgs::Imu msg;
-        FpaImuPayloadToRos(payload, msg);
+        fixposition_driver_msgs::FpaImu msg;
+        FpaImuPayloadToRos(payload, msg.data);
+        msg.bias_comp = payload.bias_comp;
+        msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status);
         pub.publish(msg);
     }
 }
 
 void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        sensor_msgs::Imu msg;
-        FpaImuPayloadToRos(payload, msg);
+        fixposition_driver_msgs::FpaImu msg;
+        FpaImuPayloadToRos(payload, msg.data);
+        msg.bias_comp = payload.bias_comp;
+        msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status);
         pub.publish(msg);
     }
 }

fixposition_driver_ros1/src/data_to_ros1.cpp

@@ -222,15 +222,15 @@ bool FixpositionDriverNode::StartNode() {
 
     // FP_A-RAWIMU
     if (params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) {
-        _PUB(rawimu_pub_, sensor_msgs::Imu, output_ns + "/fpa/rawimu", 5);
+        _PUB(rawimu_pub_, fixposition_driver_msgs::FpaImu, output_ns + "/fpa/rawimu", 5);
         driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
             PublishFpaRawimu(dynamic_cast<const fpa::FpaRawimuPayload&>(payload), rawimu_pub_);
         });
     }
 
     // FP_A-CORRIMU
     if (params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) {
-        _PUB(corrimu_pub_, sensor_msgs::Imu, output_ns + "/fpa/corrimu", 5);
+        _PUB(corrimu_pub_, fixposition_driver_msgs::FpaImu, output_ns + "/fpa/corrimu", 5);
         driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
             PublishFpaCorrimu(dynamic_cast<const fpa::FpaCorrimuPayload&>(payload), corrimu_pub_);
         });

fixposition_driver_ros1/src/fixposition_driver_node.cpp

@@ -63,9 +63,9 @@ void PublishFpaTp(const fpsdk::common::parser::fpa::FpaTpPayload& payload,
 void PublishFpaText(const fpsdk::common::parser::fpa::FpaTextPayload& payload,
                     rclcpp::Publisher<fpmsgs::FpaText>::SharedPtr& pub);
 void PublishFpaRawimu(const fpsdk::common::parser::fpa::FpaRawimuPayload& payload,
-                      rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub);
+                      rclcpp::Publisher<fpmsgs::FpaImu>::SharedPtr& pub);
 void PublishFpaCorrimu(const fpsdk::common::parser::fpa::FpaCorrimuPayload& payload,
-                       rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub);
+                       rclcpp::Publisher<fpmsgs::FpaImu>::SharedPtr& pub);
 
 bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* header,
                             const fpsdk::common::parser::novb::NovbBestgnsspos* payload,

fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp

@@ -105,9 +105,9 @@ class FixpositionDriverNode {
     //! Euler angles pitch-roll as estimated from the IMU in local horizontal
     rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_imu_pub_;
     // - IMU
-    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr rawimu_pub_;   //!< Raw IMU data in IMU frame
-    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr corrimu_pub_;  //!< Bias corrected IMU data in IMU frame
-    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;   //!< Bias corrected IMU data in POI frame
+    rclcpp::Publisher<fpmsgs::FpaImu>::SharedPtr rawimu_pub_;         //!< Raw IMU data in IMU frame
+    rclcpp::Publisher<fpmsgs::FpaImu>::SharedPtr corrimu_pub_;        //!< Bias corrected IMU data in IMU frame
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;  //!< Bias corrected IMU data in POI frame
     // - GNSS
     rclcpp::Publisher<fpmsgs::NmeaEpoch>::SharedPtr nmea_epoch_pub_;                 //!< NMEA epoch data
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss1_pub_;  //!< GNSS1 position and status

fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp

@@ -37,6 +37,7 @@
 #include <fixposition_driver_msgs/msg/fpa_eoe.hpp>
 #include <fixposition_driver_msgs/msg/fpa_gnssant.hpp>
 #include <fixposition_driver_msgs/msg/fpa_gnsscorr.hpp>
+#include <fixposition_driver_msgs/msg/fpa_imu.hpp>
 #include <fixposition_driver_msgs/msg/fpa_imubias.hpp>
 #include <fixposition_driver_msgs/msg/fpa_llh.hpp>
 #include <fixposition_driver_msgs/msg/fpa_odomenu.hpp>

fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp

@@ -403,19 +403,22 @@ static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::ms
     }
 }
 
-void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub) {
+void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, rclcpp::Publisher<fpmsgs::FpaImu>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        sensor_msgs::msg::Imu msg;
-        FpaImuPayloadToRos(payload, msg);
+        fpmsgs::FpaImu msg;
+        FpaImuPayloadToRos(payload, msg.data);
+        msg.bias_comp = payload.bias_comp;
+        msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status);
         pub->publish(msg);
     }
 }
 
-void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload,
-                       rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub) {
+void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload, rclcpp::Publisher<fpmsgs::FpaImu>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        sensor_msgs::msg::Imu msg;
-        FpaImuPayloadToRos(payload, msg);
+        fpmsgs::FpaImu msg;
+        FpaImuPayloadToRos(payload, msg.data);
+        msg.bias_comp = payload.bias_comp;
+        msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status);
         pub->publish(msg);
     }
 }