From 9ae321ee139f61b2a0069a525731556efcfdbf31 Mon Sep 17 00:00:00 2001 From: fgarciacardenas <47540719+fgarciacardenas@users.noreply.github.com> Date: Fri, 30 Aug 2024 17:54:23 +0200 Subject: [PATCH] Removed bug in odom_llh when Fusion is not initialized (#67) * Removed bug in odom_llh when Fusion is not initialized --------- Co-authored-by: Facundo Garcia --- fixposition_driver_ros1/src/data_to_ros1.cpp | 32 ++++++++++++-------- fixposition_driver_ros2/src/data_to_ros2.cpp | 32 ++++++++++++-------- 2 files changed, 40 insertions(+), 24 deletions(-) diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 49ee507..48118e6 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -530,20 +530,28 @@ void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) { msg.header.frame_id = data.odom.child_frame_id; // Populate LLH position - const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position); - msg.latitude = RadToDeg(llh_pos(0)); - msg.longitude = RadToDeg(llh_pos(1)); - msg.altitude = llh_pos(2); - - // Populate LLH covariance - const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position); - const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); - Eigen::Map> cov_map = Eigen::Map>(msg.position_covariance.data()); - cov_map = p_cov_l; - msg.position_covariance_type = 3; + + if (data.odom.pose.position.isZero()) { + msg.latitude = 0; + msg.longitude = 0; + msg.altitude = 0; + msg.position_covariance_type = 0; + cov_map = Eigen::Matrix3d::Zero(); + } else { + const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position); + msg.latitude = RadToDeg(llh_pos(0)); + msg.longitude = RadToDeg(llh_pos(1)); + msg.altitude = llh_pos(2); + + // Populate LLH covariance + const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); + const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position); + const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); + cov_map = p_cov_l; + msg.position_covariance_type = 3; + } // Populate LLH status int status_flag = std::max(data.gnss1_status, data.gnss2_status); diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index d60f80e..a6cf6bc 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -528,20 +528,28 @@ void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher> cov_map = Eigen::Map>(msg.position_covariance.data()); - cov_map = p_cov_l; - msg.position_covariance_type = 3; + + if (data.odom.pose.position.isZero()) { + msg.latitude = 0; + msg.longitude = 0; + msg.altitude = 0; + msg.position_covariance_type = 0; + cov_map = Eigen::Matrix3d::Zero(); + } else { + const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position); + msg.latitude = RadToDeg(llh_pos(0)); + msg.longitude = RadToDeg(llh_pos(1)); + msg.altitude = llh_pos(2); + + // Populate LLH covariance + const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); + const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position); + const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); + cov_map = p_cov_l; + msg.position_covariance_type = 3; + } // Populate LLH status int status_flag = std::max(data.gnss1_status, data.gnss2_status);