Added odometry_enu_smooth topic

fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp

@@ -103,6 +103,7 @@ struct OdometryData {
     PoseWithCovData pose;
     TwistWithCovData twist;
     bool SetFromFpaOdomPayload(const fpsdk::common::parser::fpa::FpaOdomPayload& payload);
+    bool ConvertToEnu(const TfData& tf_ecef_enu0);
 };
 
 /**

fixposition_driver_lib/src/helper.cpp

@@ -18,6 +18,7 @@
 #include <fpsdk_common/logging.hpp>
 #include <fpsdk_common/parser/crc.hpp>
 #include <fpsdk_common/string.hpp>
+#include <fpsdk_common/trafo.hpp>
 #include <fpsdk_common/types.hpp>
 #include <fpsdk_common/utils.hpp>
 
@@ -148,6 +149,42 @@ bool OdometryData::SetFromFpaOdomPayload(const fpa::FpaOdomPayload& payload) {
 
 // ---------------------------------------------------------------------------------------------------------------------
 
+bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) {
+    // Check that TF data is valid
+    if (!tf_ecef_enu0.valid) {
+        return false;
+    }
+
+    // Extract data from the odometry message
+    const Eigen::Vector3d t_ecef_body = pose.position;
+    const Eigen::Quaterniond q_ecef_body = pose.orientation;
+    const Eigen::Matrix<double, 6, 6> cov_ecef = pose.cov;
+
+    // Extract data from the TF message (using the arrow operator)
+    const Eigen::Vector3d t_ecef_enu0 = tf_ecef_enu0.translation;
+    const Eigen::Quaterniond q_ecef_enu0 = tf_ecef_enu0.rotation;
+    const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0.toRotationMatrix();
+
+    // Convert position in ECEF into position in ENU
+    const Eigen::Vector3d t_enu_body =
+        fpsdk::common::trafo::TfEnuEcef(t_ecef_body, fpsdk::common::trafo::TfWgs84LlhEcef(t_ecef_enu0));
+    const Eigen::Quaterniond q_enu_body = q_ecef_enu0.inverse() * q_ecef_body;
+
+    // Convert covariance matrix to ENU
+    Eigen::Matrix<double, 6, 6> cov_enu = Eigen::Matrix<double, 6, 6>::Zero();
+    cov_enu.topLeftCorner(3, 3) = rot_ecef_enu0 * cov_ecef.topLeftCorner(3, 3) * rot_ecef_enu0.transpose();
+    cov_enu.bottomRightCorner(3, 3) = rot_ecef_enu0 * cov_ecef.bottomRightCorner(3, 3) * rot_ecef_enu0.transpose();
+
+    // Repopulate odometry data
+    pose.position = t_enu_body;
+    pose.orientation = q_enu_body;
+    pose.cov = cov_enu;
+
+    return true;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
 bool TfData::SetFromFpaTfPayload(const fpa::FpaTfPayload& payload) {
     bool ok = true;
     if (payload.gps_time.week.valid && payload.gps_time.tow.valid) {

fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp

@@ -35,8 +35,9 @@ void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStampe
 void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg);
 
 void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub);
-void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub);
-void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload,
+void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_,
+                               ros::Publisher& pub);
+void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_,
                                      ros::Publisher& pub);
 void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub);
 void PublishFpaOdomenuVector3Stamped(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub);
@@ -73,6 +74,7 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, ros::Publishe
 void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub);
 void PublishOdometryData(const OdometryData& data, ros::Publisher& pub);
 void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub);
+void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, ros::Publisher& pub);
 void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub);
 
 /* ****************************************************************************************************************** */

fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp

@@ -88,10 +88,11 @@ class FixpositionDriverNode {
     // - NOV_B messages
     ros::Publisher novb_inspvax_pub_;  //!< NOV_B-INSPVAX message
     // - Odometry
-    ros::Publisher odometry_ecef_pub_;    //!< ECEF odometry
-    ros::Publisher odometry_enu_pub_;     //!< ENU odometry
-    ros::Publisher odometry_llh_pub_;     //!< LLH odometry
-    ros::Publisher odometry_smooth_pub_;  //!< Smooth Odometry (ECEF)
+    ros::Publisher odometry_ecef_pub_;        //!< ECEF odometry
+    ros::Publisher odometry_enu_pub_;         //!< ENU odometry
+    ros::Publisher odometry_smooth_pub_;      //!< Smooth Odometry (ECEF)
+    ros::Publisher odometry_enu_smooth_pub_;  //!< Smooth Odometry (ENU)
+    ros::Publisher odometry_llh_pub_;         //!< LLH odometry
     // - Fusion
     ros::Publisher fusion_epoch_pub_;  //!< Fusion epoch data
     // - Orientation
@@ -106,8 +107,9 @@ class FixpositionDriverNode {
     ros::Publisher navsatfix_gnss2_pub_;  //!< GNSS2 position and status
     ros::Publisher nmea_epoch_pub_;       //!< NMEA epoch data
     // - Other
-    ros::Publisher jump_pub_;  //!< Jump warning topic
-    ros::Publisher raw_pub_;   //!< Raw messages topic
+    ros::Publisher jump_pub_;   //!< Jump warning topic
+    ros::Publisher raw_pub_;    //!< Raw messages topic
+    ros::Publisher datum_pub_;  //!< WGS84 datum topic
 
     // ROS subscribers
     ros::Subscriber ws_sub_;       //!< Wheelspeed input subscriber
@@ -132,6 +134,7 @@ class FixpositionDriverNode {
         std::unique_ptr<geometry_msgs::TransformStamped> enu0_poi_;
     };
     Tfs tfs_;
+    std::unique_ptr<TfData> ecef_enu0_tf_;
 
     void ProcessTfData(const TfData& tf_data);
     void ProcessOdometryData(const OdometryData& odometry_data);

fixposition_driver_ros1/launch/config.yaml

@@ -6,7 +6,7 @@ stream: tcpcli://10.0.2.1:21000
 # Wait time in [s] to attempt reconnecting after connection lost
 reconnect_delay: 5.0
 # Delay warning threshold [s]. Note that this only works if your system time is synced to the VRTK2.
-delay_warning: 0.01
+delay_warning: 0.03
 
 # Messages that should be used by the driver. Note that the sensor must be configured accordingly for the correct
 # port used in the "connection" above.

fixposition_driver_ros1/src/data_to_ros1.cpp

@@ -135,11 +135,15 @@ void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, ros::Publisher& pub)
 
 // ---------------------------------------------------------------------------------------------------------------------
 
-void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) {
+void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
         sensor_msgs::Imu msg;
         msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
-        msg.header.frame_id = ODOMETRY_FRAME_ID;
+        if (nav2_mode_) {
+            msg.header.frame_id = "vrtk_link";
+        } else {
+            msg.header.frame_id = ODOMETRY_FRAME_ID;
+        }
         FpaFloat3ToVector3(payload.acc, msg.linear_acceleration);
         FpaFloat3ToVector3(payload.rot, msg.angular_velocity);
         pub.publish(msg);
@@ -148,11 +152,15 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, ros::Publ
 
 // ---------------------------------------------------------------------------------------------------------------------
 
-void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) {
+void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
         sensor_msgs::NavSatFix msg;
         msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
-        msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID;
+        if (nav2_mode_) {
+            msg.header.frame_id = "vrtk_link";
+        } else {
+            msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID;
+        }
 
         // Populate LLH position
         PoseWithCovData pose;
@@ -178,11 +186,16 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, ros
         }
 
         // Populate LLH status
-        const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix);
-        msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix);
-        if (msg.status.status != msg.status.STATUS_NO_FIX) {
-            msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS |
-                                  msg.status.SERVICE_GALILEO);
+        if (nav2_mode_) {
+            msg.status.status = 2;
+            msg.status.service = 15;
+        } else {
+            const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix);
+            msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix);
+            if (msg.status.status != msg.status.STATUS_NO_FIX) {
+                msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS |
+                                      msg.status.SERVICE_GALILEO);
+            }
         }
 
         // Publish message
@@ -800,6 +813,31 @@ void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub)
 
 // ---------------------------------------------------------------------------------------------------------------------
 
+void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        sensor_msgs::NavSatFix msg;
+        msg.header.stamp = stamp;
+        msg.header.frame_id = "vrtk_link";
+
+        // Populate LLH position
+        const Eigen::Vector3d position = {payload.x, payload.y, payload.z};
+        const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(position);
+        msg.latitude = math::RadToDeg(llh_pos(0));
+        msg.longitude = math::RadToDeg(llh_pos(1));
+        msg.altitude = llh_pos(2);
+
+        // Populate status
+        msg.status.status = 2;
+        msg.status.service = 15;
+        msg.position_covariance_type = 3;
+
+        // Publish message
+        pub.publish(msg);
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
 void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
         fixposition_driver_msgs::FusionEpoch msg;

fixposition_driver_ros1/src/fixposition_driver_node.cpp

@@ -79,8 +79,8 @@ bool FixpositionDriverNode::StartNode() {
         driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
             auto odometry_payload = dynamic_cast<const fpa::FpaOdometryPayload&>(payload);
             PublishFpaOdometry(odometry_payload, fpa_odometry_pub_);
-            PublishFpaOdometryDataImu(odometry_payload, poiimu_pub_);
-            PublishFpaOdometryDataNavSatFix(odometry_payload, odometry_llh_pub_);
+            PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_);
+            PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_);
             OdometryData odometry_data;
             odometry_data.SetFromFpaOdomPayload(odometry_payload);
             PublishOdometryData(odometry_data, odometry_ecef_pub_);
@@ -93,14 +93,30 @@ bool FixpositionDriverNode::StartNode() {
     if (params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) {
         _PUB(fpa_odomsh_pub_, fixposition_driver_msgs::FpaOdomsh, output_ns + "/fpa/odomsh", 5);
         _PUB(odometry_smooth_pub_, nav_msgs::Odometry, output_ns + "/odometry_smooth", 5);
+        _PUB(odometry_enu_smooth_pub_, nav_msgs::Odometry, output_ns + "/odometry_enu_smooth", 5);
         driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
             auto odomsh_payload = dynamic_cast<const fpa::FpaOdomshPayload&>(payload);
             PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_);
             OdometryData odometry_data;
             odometry_data.SetFromFpaOdomPayload(odomsh_payload);
+
+            // Update frames for Nav2
+            if (params_.nav2_mode_) {
+                odometry_data.frame_id = "odom";
+                odometry_data.child_frame_id = "vrtk_link";
+            }
+
             PublishOdometryData(odometry_data, odometry_smooth_pub_);
             ProcessOdometryData(odometry_data);
             fusion_epoch_data_.CollectFpaOdomsh(odomsh_payload);
+
+            // Convert message to ENU
+            if (ecef_enu0_tf_) {
+                bool enu_valid = odometry_data.ConvertToEnu(*ecef_enu0_tf_);
+                if (enu_valid) {
+                    PublishOdometryData(odometry_data, odometry_enu_smooth_pub_);
+                }
+            }
         });
     }
 
@@ -115,6 +131,13 @@ bool FixpositionDriverNode::StartNode() {
             PublishFpaOdomenuVector3Stamped(odomenu_payload, eul_pub_);
             OdometryData odometry_data;
             odometry_data.SetFromFpaOdomPayload(odomenu_payload);
+
+            // Update frames for Nav2
+            if (params_.nav2_mode_) {
+                odometry_data.frame_id = "map";
+                odometry_data.child_frame_id = "vrtk_link";
+            }
+
             PublishOdometryData(odometry_data, odometry_enu_pub_);
             ProcessOdometryData(odometry_data);
             fusion_epoch_data_.CollectFpaOdomenu(odomenu_payload);
@@ -368,6 +391,11 @@ bool FixpositionDriverNode::StartNode() {
         _PUB(jump_pub_, fixposition_driver_msgs::CovWarn, output_ns + "/extras/jump", 5);
     }
 
+    // WGS84 datum message
+    if (params_.nav2_mode_) {
+        _PUB(datum_pub_, sensor_msgs::NavSatFix, output_ns + "/datum", 5);
+    }
+
     // Subscribe to correction data input
     if (!params_.corr_topic_.empty()) {
         _SUB(corr_sub_, rtcm_msgs::Message, params_.corr_topic_, 100, [this](const rtcm_msgs::MessageConstPtr& msg) {
@@ -457,6 +485,7 @@ void FixpositionDriverNode::StopNode() {
     odometry_enu_pub_.shutdown();
     odometry_llh_pub_.shutdown();
     odometry_smooth_pub_.shutdown();
+    odometry_enu_smooth_pub_.shutdown();
     // - Orientation
     eul_pub_.shutdown();
     eul_imu_pub_.shutdown();
@@ -472,6 +501,7 @@ void FixpositionDriverNode::StopNode() {
     // - Other
     jump_pub_.shutdown();
     raw_pub_.shutdown();
+    datum_pub_.shutdown();
 
     // Stop input message subscribers
     ws_sub_.shutdown();
@@ -482,6 +512,14 @@ void FixpositionDriverNode::StopNode() {
 // ---------------------------------------------------------------------------------------------------------------------
 
 void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) {
+    // Check if TF is valid
+    if (tf_data.rotation.w() == 0 && tf_data.rotation.vec().isZero()) {
+        ROS_WARN_THROTTLE(10.0, "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s",
+                          tf_data.frame_id.c_str(), tf_data.child_frame_id.c_str());
+        return;
+    }
+
+    // Generate TF message
     geometry_msgs::TransformStamped tf;
     TfDataToTransformStamped(tf_data, tf);
 
@@ -498,7 +536,6 @@ void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) {
         imu_ypr.header.frame_id = "FP_POI";
         tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector);
         eul_imu_pub_.publish(imu_ypr);
-
     }
     // FP_POI -> FP_POISH
     else if ((tf.child_frame_id == "FP_POISH") && (tf.header.frame_id == "FP_POI")) {
@@ -512,6 +549,7 @@ void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) {
     // FP_ECEF -> FP_ENU0
     else if ((tf.child_frame_id == "FP_ENU0") && (tf.header.frame_id == "FP_ECEF")) {
         static_br_.sendTransform(tf);
+        ecef_enu0_tf_ = std::make_unique<TfData>(tf_data);
         // Store TF if Nav2 mode is enabled
         if (params_.nav2_mode_) {
             std::unique_lock<std::mutex> lock(tfs_.mutex_);
@@ -587,9 +625,19 @@ void FixpositionDriverNode::PublishNav2Tf() {
         return;
     }
 
-    // Publish FP_ECEF -> map
-    tfs_.ecef_enu0_->child_frame_id = "map";
-    static_br_.sendTransform(*tfs_.ecef_enu0_);
+    // Publish a static identity transform from FP_ENU0 to map
+    geometry_msgs::TransformStamped static_transform;
+    static_transform.header.stamp = tfs_.ecef_enu0_->header.stamp;
+    static_transform.header.frame_id = "FP_ENU0";
+    static_transform.child_frame_id = "map";
+    static_transform.transform.translation.x = 0.0;
+    static_transform.transform.translation.y = 0.0;
+    static_transform.transform.translation.z = 0.0;
+    static_transform.transform.rotation.w = 1.0;
+    static_transform.transform.rotation.x = 0.0;
+    static_transform.transform.rotation.y = 0.0;
+    static_transform.transform.rotation.z = 0.0;
+    static_br_.sendTransform(static_transform);
 
     // Compute FP_ENU0 -> FP_POISH
     // Extract translation and rotation from ECEFENU0
@@ -624,21 +672,24 @@ void FixpositionDriverNode::PublishNav2Tf() {
 
     // Create a new TransformStamped message
     geometry_msgs::TransformStamped tfs_odom;
-    tfs_odom.header.stamp = ros::Time::now();
+    tfs_odom.header.stamp = tfs_.enu0_poi_->header.stamp;
     tfs_odom.header.frame_id = "map";
     tfs_odom.child_frame_id = "odom";
     tfs_odom.transform = tf2::toMsg(tf_combined);
     tf_br_.sendTransform(tfs_odom);
 
-    // Publish odom -> base_link
+    // Publish odom -> vrtk_link
     geometry_msgs::TransformStamped tf_odom_base;
-    tf_odom_base.header.stamp = ros::Time::now();
+    tf_odom_base.header.stamp = tfs_.enu0_poi_->header.stamp;
     tf_odom_base.header.frame_id = "odom";
-    tf_odom_base.child_frame_id = "base_link";
+    tf_odom_base.child_frame_id = "vrtk_link";
     tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH);
 
     // Send the transform
     tf_br_.sendTransform(tf_odom_base);
+
+    // Publish WGS84 datum
+    PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_);
 }
 
 /* ****************************************************************************************************************** */