From d30c8c507f71ca889e1458bf02518397e797fdf7 Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Thu, 21 Sep 2023 12:35:39 -0400 Subject: [PATCH 1/2] adding check for if the final gps odom has been transformed. --- include/robot_localization/navsat_transform.h | 5 +- src/navsat_transform.cpp | 64 ++++++++++--------- 2 files changed, 38 insertions(+), 31 deletions(-) diff --git a/include/robot_localization/navsat_transform.h b/include/robot_localization/navsat_transform.h index 0b47419f..8c277d56 100644 --- a/include/robot_localization/navsat_transform.h +++ b/include/robot_localization/navsat_transform.h @@ -108,8 +108,10 @@ class NavSatTransform //! @brief Given the pose of the navsat sensor in the world frame, removes the offset from the vehicle's centroid //! and returns the world-frame pose of said centroid. + //! @param[out] gps_odom The world-frame pose of base_link + //! @return whether robot_odom_pose was transformed //! - void getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, + bool getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, tf2::Transform &robot_odom_pose, const ros::Time &transform_time); @@ -401,3 +403,4 @@ class NavSatTransform } // namespace RobotLocalization #endif // ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H + diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index fac581a0..b2b424ea 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -564,7 +564,7 @@ namespace RobotLocalization } } - void NavSatTransform::getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, + bool NavSatTransform::getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, tf2::Transform &robot_odom_pose, const ros::Time &transform_time) { @@ -610,6 +610,7 @@ namespace RobotLocalization ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << base_link_frame_id_ << "->" << gps_frame_id_ << " transform. Will not remove offset of navsat device from robot's origin."); } + return can_transform; } void NavSatTransform::gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg) @@ -867,42 +868,44 @@ namespace RobotLocalization // Want the pose of the vehicle origin, not the GPS tf2::Transform transformed_cartesian_robot; - getRobotOriginWorldPose(transformed_cartesian_gps, transformed_cartesian_robot, gps_odom.header.stamp); - - // Rotate the covariance as well - tf2::Matrix3x3 rot(cartesian_world_transform_.getRotation()); - Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE); - rot_6d.setIdentity(); - - for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + bool is_transformed = getRobotOriginWorldPose(transformed_cartesian_gps, transformed_cartesian_robot, gps_odom.header.stamp); + if (is_transformed) { - rot_6d(rInd, 0) = rot.getRow(rInd).getX(); - rot_6d(rInd, 1) = rot.getRow(rInd).getY(); - rot_6d(rInd, 2) = rot.getRow(rInd).getZ(); - rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); - rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); - rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); - } + // Rotate the covariance as well + tf2::Matrix3x3 rot(cartesian_world_transform_.getRotation()); + Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE); + rot_6d.setIdentity(); - // Rotate the covariance - latest_cartesian_covariance_ = rot_6d * latest_cartesian_covariance_.eval() * rot_6d.transpose(); + for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + { + rot_6d(rInd, 0) = rot.getRow(rInd).getX(); + rot_6d(rInd, 1) = rot.getRow(rInd).getY(); + rot_6d(rInd, 2) = rot.getRow(rInd).getZ(); + rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); + rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); + rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); + } - // Now fill out the message. Set the orientation to the identity. - tf2::toMsg(transformed_cartesian_robot, gps_odom.pose.pose); - gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z); + // Rotate the covariance + latest_cartesian_covariance_ = rot_6d * latest_cartesian_covariance_.eval() * rot_6d.transpose(); - // Copy the measurement's covariance matrix so that we can rotate it later - for (size_t i = 0; i < POSE_SIZE; i++) - { - for (size_t j = 0; j < POSE_SIZE; j++) + // Now fill out the message. Set the orientation to the identity. + tf2::toMsg(transformed_cartesian_robot, gps_odom.pose.pose); + gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z); + + // Copy the measurement's covariance matrix so that we can rotate it later + for (size_t i = 0; i < POSE_SIZE; i++) { - gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_cartesian_covariance_(i, j); + for (size_t j = 0; j < POSE_SIZE; j++) + { + gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_cartesian_covariance_(i, j); + } } - } - // Mark this GPS as used - gps_updated_ = false; - new_data = true; + // Mark this GPS as used + gps_updated_ = false; + new_data = true; + } } return new_data; @@ -964,3 +967,4 @@ namespace RobotLocalization } } // namespace RobotLocalization + From f7c9f36431ac0b1351146fbb2af0f6cbaf6130e7 Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Thu, 21 Sep 2023 12:38:47 -0400 Subject: [PATCH 2/2] changelog update --- CHANGELOG.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 592a5f86..c715fc68 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -22,6 +22,7 @@ Forthcoming * Adding Pipeline * Throttling GNSS Origin data warning * Added precision to origin output +* Added check to prevent null GPS odometry from being published due to missing transform 2.7.3 (2021-07-23) ------------------