From 319bbdcb7c4827ab8d9c3029a796f29bf99f9342 Mon Sep 17 00:00:00 2001 From: Karl Schulz Date: Mon, 26 May 2025 08:39:43 +0200 Subject: [PATCH 1/2] Wait for setDatum call before processing incoming messages --- .../robot_localization/navsat_transform.hpp | 5 ++ src/navsat_transform.cpp | 51 +++++++++++++++---- 2 files changed, 45 insertions(+), 11 deletions(-) diff --git a/include/robot_localization/navsat_transform.hpp b/include/robot_localization/navsat_transform.hpp index 69bcdd75..46f9ad87 100644 --- a/include/robot_localization/navsat_transform.hpp +++ b/include/robot_localization/navsat_transform.hpp @@ -421,6 +421,11 @@ class NavSatTransform : public rclcpp::Node { */ bool use_manual_datum_; + /** + * @brief Whether we got a datum from the datum service or parameters + */ + bool datum_is_set_; + /** * @brief Whether we get the transform's yaw from the odometry or IMU source */ diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index aa70be86..7bd40a0c 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -82,6 +82,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) use_local_cartesian_(false), force_user_utm_(false), use_manual_datum_(false), + datum_is_set_(false), use_odometry_yaw_(false), cartesian_broadcaster_(*this), utm_meridian_convergence_(0.0), @@ -168,17 +169,20 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) datum_lat = datum_vals[0]; datum_lon = datum_vals[1]; datum_yaw = datum_vals[2]; - } - auto request = std::make_shared(); - request->geo_pose.position.latitude = datum_lat; - request->geo_pose.position.longitude = datum_lon; - request->geo_pose.position.altitude = 0.0; - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, datum_yaw); - request->geo_pose.orientation = tf2::toMsg(quat); - auto response = std::make_shared(); - datumCallback(request, response); + auto request = std::make_shared(); + request->geo_pose.position.latitude = datum_lat; + request->geo_pose.position.longitude = datum_lon; + request->geo_pose.position.altitude = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, datum_yaw); + request->geo_pose.orientation = tf2::toMsg(quat); + auto response = std::make_shared(); + datumCallback(request, response); + } else { + RCLCPP_INFO( + this->get_logger(), "No datum parameter given, waiting to SetDatum call."); + } } auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1)); @@ -377,8 +381,9 @@ bool NavSatTransform::datumCallback( // we are using a datum from now on, and we want other methods to not attempt // to transform the values we are specifying here. use_manual_datum_ = true; - + datum_is_set_ = true; transform_good_ = false; + RCLCPP_INFO(this->get_logger(), "Datum set."); return true; } @@ -423,6 +428,10 @@ bool NavSatTransform::toLLCallback( const std::shared_ptr request, std::shared_ptr response) { + if (use_manual_datum_ && !datum_is_set_){ + // We have to wait for a call to SetDatum before computing anything. + return false; + } if (!transform_good_) { return false; } @@ -439,6 +448,10 @@ bool NavSatTransform::fromLLCallback( const std::shared_ptr request, std::shared_ptr response) { + if (use_manual_datum_ && !datum_is_set_){ + // We have to wait for a call to SetDatum before computing anything. + return false; + } try { response->map_point = fromLL(request->ll_point); } catch(const std::runtime_error & e) { @@ -452,6 +465,10 @@ bool NavSatTransform::fromLLArrayCallback( const std::shared_ptr request, std::shared_ptr response) { + if (use_manual_datum_ && !datum_is_set_){ + // We have to wait for a call to SetDatum before computing anything. + return false; + } decltype(response->map_points) converted_points; converted_points.reserve(request->ll_points.size()); @@ -692,6 +709,10 @@ void NavSatTransform::getRobotOriginWorldPose( void NavSatTransform::gpsFixCallback( const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + if (use_manual_datum_ && !datum_is_set_){ + // We have to wait for a call to SetDatum before computing anything. + return; + } gps_frame_id_ = msg->header.frame_id; if (gps_frame_id_.empty()) { @@ -753,6 +774,10 @@ void NavSatTransform::gpsFixCallback( void NavSatTransform::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { + if (use_manual_datum_ && !datum_is_set_){ + // We have to wait for a call to SetDatum before computing anything. + return; + } // We need the baseLinkFrameId_ from the odometry message, so // we need to wait until we receive it. if (has_transform_odom_) { @@ -805,6 +830,10 @@ void NavSatTransform::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) void NavSatTransform::odomCallback( const nav_msgs::msg::Odometry::SharedPtr msg) { + if (use_manual_datum_ && !datum_is_set_){ + // We have to wait for a call to SetDatum before computing anything. + return; + } world_frame_id_ = msg->header.frame_id; base_link_frame_id_ = msg->child_frame_id; From bfe23c4dabdebdad37ed5a8d589d165a78bae45c Mon Sep 17 00:00:00 2001 From: Karl Schulz Date: Tue, 27 May 2025 12:19:11 +0200 Subject: [PATCH 2/2] Catch and fix race condition with fix arriving before TF is computed --- src/navsat_transform.cpp | 34 +++++++++++++++++++++------------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index 7bd40a0c..a1553b5c 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -257,7 +257,7 @@ void NavSatTransform::computeTransform() // When using manual datum, wait for the receive of odometry message so // that the base frame and world frame names can be set before // the manual datum pose is set. This must be done prior to the transform computation. - if (!transform_good_ && has_transform_odom_ && use_manual_datum_) { + if (!transform_good_ && has_transform_odom_ && use_manual_datum_ && datum_is_set_) { setManualDatum(); } @@ -269,6 +269,7 @@ void NavSatTransform::computeTransform() { // The UTM pose we have is given at the location of the GPS sensor on the // robot. We need to get the UTM pose of the robot's origin. + RCLCPP_INFO(this->get_logger(), "Computing utm->odom TF."); tf2::Transform transform_cartesian_pose_corrected; if (!use_manual_datum_) { getRobotOriginCartesianPose( @@ -389,6 +390,7 @@ bool NavSatTransform::datumCallback( void NavSatTransform::setManualDatum() { + RCLCPP_INFO(this->get_logger(), "Setting map origin fix from manual datum value."); sensor_msgs::msg::NavSatFix fix; fix.latitude = manual_datum_geopose_.position.latitude; fix.longitude = manual_datum_geopose_.position.longitude; @@ -515,7 +517,7 @@ geometry_msgs::msg::Point NavSatTransform::fromLL( latitude, longitude, zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_); } catch (GeographicLib::GeographicErr const & e) { - RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Error in fromLL: " << e.what()); throw; } } @@ -609,7 +611,7 @@ void NavSatTransform::mapToLL( longitude); altitude = odom_as_cartesian.getOrigin().getZ(); } catch (const GeographicLib::GeographicErr & e) { - RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Error in mapToLL: " << e.what()); latitude = longitude = altitude = std::numeric_limits::quiet_NaN(); } } @@ -713,6 +715,16 @@ void NavSatTransform::gpsFixCallback( // We have to wait for a call to SetDatum before computing anything. return; } + if (use_manual_datum_ && datum_is_set_ && !transform_good_) { + // This race condition that can happen when the TF is not prepared before the first fix message arrives + RCLCPP_ERROR_THROTTLE( + this->get_logger(), + *this->get_clock(), 5000, + "Cannot compute fix to odom because TF is missing: has_transform_gps: %s, " + "has_transform_odom: %s, has_transform_imu: %s", + has_transform_gps_ ? "true" : "false", has_transform_odom_ ? "true" : "false", has_transform_imu_ ? "true" : "false"); + return; + } gps_frame_id_ = msg->header.frame_id; if (gps_frame_id_.empty()) { @@ -751,7 +763,7 @@ void NavSatTransform::gpsFixCallback( msg->latitude, msg->longitude, zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_); } catch (GeographicLib::GeographicErr const & e) { - RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Error converting fix message to odometry: " << e.what()); return; } } @@ -774,10 +786,6 @@ void NavSatTransform::gpsFixCallback( void NavSatTransform::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { - if (use_manual_datum_ && !datum_is_set_){ - // We have to wait for a call to SetDatum before computing anything. - return; - } // We need the baseLinkFrameId_ from the odometry message, so // we need to wait until we receive it. if (has_transform_odom_) { @@ -830,16 +838,16 @@ void NavSatTransform::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) void NavSatTransform::odomCallback( const nav_msgs::msg::Odometry::SharedPtr msg) { - if (use_manual_datum_ && !datum_is_set_){ - // We have to wait for a call to SetDatum before computing anything. - return; - } world_frame_id_ = msg->header.frame_id; base_link_frame_id_ = msg->child_frame_id; if (!transform_good_) { setTransformOdometry(msg); } + if (use_manual_datum_ && !datum_is_set_) { + // We have to wait for a call to SetDatum before computing anything. + return; + } tf2::fromMsg(msg->pose.pose, latest_world_pose_); latest_odom_covariance_.setZero(); @@ -990,7 +998,7 @@ void NavSatTransform::setTransformGps( msg->latitude, msg->longitude, utm_zone_, northp_, cartesian_x, cartesian_y, utm_meridian_convergence_degrees, k_tmp, set_zone); } catch (const GeographicLib::GeographicErr & e) { - RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Error computing utm->map TF: " << e.what()); return; } utm_meridian_convergence_ = utm_meridian_convergence_degrees *