From 7b3c44061e4b9a401713f23650f399462ed539b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 19 Aug 2026 10:35:35 +0200 Subject: [PATCH 1/2] Fix busy wait for TFs and NaN MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../config/example_params_summit_dual.yaml | 1 - .../example_params_summit_dual_gazebo.yaml | 1 - .../FusionLocalizer.cpp | 34 +++++++-- .../easynav_fusion_localizer/ukf_wrapper.cpp | 69 ++++++++++++------- 4 files changed, 76 insertions(+), 29 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index 40b6d005..10b65591 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -125,7 +125,6 @@ localizer_node: publish_tf: true debug: true debug_out_file: /tmp/ukf_global_debug.txt - transform_timeout: 0.1 smooth_lagged_data: true # Defines the time window in seconds to keep past states in memory. # history_length: 5.0 diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml index 7ec21016..321b4ca3 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -125,7 +125,6 @@ localizer_node: publish_tf: true debug: true debug_out_file: /tmp/ukf_global_debug.txt - transform_timeout: 0.1 smooth_lagged_data: true # Defines the time window in seconds to keep past states in memory. # history_length: 5.0 diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 8488f319..41a0c4e5 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -174,9 +174,21 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (gps_time > last_gps_stamp_[i]) { EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); last_gps_stamp_[i] = gps_time; - auto pose = - std::make_shared(navsatfix_to_pose( - gps_data[i]->data)); + std::shared_ptr pose; + try { + pose = std::make_shared( + navsatfix_to_pose(gps_data[i]->data)); + } catch (const GeographicLib::GeographicErr & e) { + // A GPS fix whose UTM projection falls outside the zone pinned + // at startup (UTM_zone_number_, from latitude_origin/ + // longitude_origin) throws here rather than clamping/wrapping. + // Discarding this one fix is safer than letting the exception + // escape update_rt() uncaught, which previously crashed the + // whole system_main process. + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, + "Discarding a GPS fix that could not be converted to UTM: %s", e.what()); + continue; + } if (!first_pose_received_) { RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); @@ -204,7 +216,21 @@ void FusionLocalizer::update_rt(NavState & nav_state) nav_msgs::msg::Odometry global_odom; if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { nav_state.set("robot_pose", global_odom); - navsat_pub_->publish(odom_to_navsatfix(global_odom)); + // odom_to_navsatfix() re-projects the *current fused estimate* back to + // lat/lon via GeographicLib::UTMUPS::Reverse, which throws if that + // estimate has drifted far enough from UTM_origin_x_/y_ to fall + // outside the pinned UTM zone (e.g. a diverging filter under a slow + // RT cycle). That's only this debug/convenience re-publish, not the + // pose estimate itself (already stored above via nav_state.set), so + // skip just this one publish rather than letting the exception + // escape update_rt() uncaught and crash system_main. + try { + navsat_pub_->publish(odom_to_navsatfix(global_odom)); + } catch (const GeographicLib::GeographicErr & e) { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, + "Could not re-project the current position estimate back to a " + "NavSatFix (likely a diverged UKF estimate): %s", e.what()); + } } } diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 5ce5bd30..0b9fe9aa 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -941,14 +941,16 @@ void UkfWrapper::loadParams() "values, it must not match the map_frame or odom_frame."); } - if (!tf_prefix_.empty()) { - // Append the tf prefix in a tf2-friendly manner - filter_utilities::appendPrefix(tf_prefix_, map_frame_id_); - filter_utilities::appendPrefix(tf_prefix_, odom_frame_id_); - filter_utilities::appendPrefix(tf_prefix_, base_link_frame_id_); - filter_utilities::appendPrefix(tf_prefix_, base_link_output_frame_id_); - filter_utilities::appendPrefix(tf_prefix_, world_frame_id_); - } + // NOTE: no tf_prefix_ re-prefixing here (unlike upstream robot_localization, + // which this code is adapted from): map_frame_id_/odom_frame_id_/ + // base_link_frame_id_/base_link_output_frame_id_/world_frame_id_ above are + // read from RTTFBuffer's TFInfo (tf_info.map_frame etc.), which + // RTTFBuffer::set_tf_info() (easynav_common/RTTFBuffer.hpp) already + // prefixes with tf_prefix once. Appending it again here produced doubled + // frame ids ("robot_1/robot_1/odom"), which never match the + // single-prefixed frames robot_state_publisher/ros2_control actually + // publish, so the global filter's TF lookups always failed and + // map->odom was never broadcast. // Whether we're publshing the world_frame->base_link_frame transform publish_transform_ = parent_node_->declare_parameter(param_prefix + "publish_tf", true); @@ -965,9 +967,16 @@ void UkfWrapper::loadParams() double offset_tmp = parent_node_->declare_parameter(param_prefix + "transform_time_offset", 0.0); tf_time_offset_ = rclcpp::Duration::from_seconds(offset_tmp); - // Transform timeout - double timeout_tmp = parent_node_->declare_parameter(param_prefix + "transform_timeout", 0.0); - tf_timeout_ = rclcpp::Duration::from_seconds(timeout_tmp); + // Transform timeout: intentionally NOT a parameter. preparePose()/ + // prepareTwist() call ros_filter_utilities::lookupTransformSafe(..., + // tf_timeout_, ...) from odometryCallback/accelerationCallback, which run + // inside easynav_system's SCHED_FIFO real-time executor thread (rt_cbg, + // see loadParams()'s "Get callback_group" above). Any nonzero timeout here + // makes that RT thread block synchronously on tf2 -- with map/odom + // momentarily unavailable this alone inflated a 5ms RT cycle to ~200ms. + // Keep this hardcoded at zero (fail-fast, never wait) so it can't be + // reintroduced via config. + tf_timeout_ = rclcpp::Duration(0, 0); // Update frequency and sensor timeout frequency_ = parent_node_->declare_parameter(param_prefix + "frequency", 30.0); @@ -2332,8 +2341,32 @@ void UkfWrapper::periodicUpdate() auto filtered_position = std::make_unique(); bool corrected_data = false; + bool got_odometry = getFilteredOdometryMessage(filtered_position.get()); + // Also gates the acceleration publish below, since getFilteredAccelMessage() + // reads the same (possibly-NaN) filter_.getState(). + bool filter_state_valid = true; + + if (got_odometry) { + // Validate *before* touching world_base_link_trans_msg_/broadcasting/ + // publishing: this used to run after those side effects and only log, + // so a NaN-diverged filter_ (e.g. an ill-conditioned covariance driving + // the UKF's internal Cholesky decomposition to produce garbage) kept + // broadcasting a NaN map->odom transform forever, with TF silently + // dropping every one of those broadcasts and no way to recover short of + // restarting system_main. Resetting here makes the filter reinitialize + // cleanly from the next valid measurement instead. + if (!validateFilterOutput(filtered_position.get())) { + RCLCPP_ERROR( + parent_node_->get_logger(), + "Critical Error, NaNs were detected in the output state of the filter. " + "This was likely due to poorly coniditioned process, noise, or sensor " + "covariances. Resetting the filter."); + reset(); + filter_state_valid = false; + } + } - if (getFilteredOdometryMessage(filtered_position.get())) { + if (got_odometry && filter_state_valid) { world_base_link_trans_msg_.header.stamp = static_cast(filtered_position->header.stamp) + tf_time_offset_; world_base_link_trans_msg_.header.frame_id = @@ -2350,16 +2383,6 @@ void UkfWrapper::periodicUpdate() world_base_link_trans_msg_.transform.rotation = filtered_position->pose.pose.orientation; - // The filtered_position is the message containing the state and covariances: - // nav_msgs Odometry - if (!validateFilterOutput(filtered_position.get())) { - RCLCPP_ERROR( - parent_node_->get_logger(), - "Critical Error, NaNs were detected in the output state of the filter. " - "This was likely due to poorly coniditioned process, noise, or sensor " - "covariances."); - } - // If we're trying to publish with the same time stamp, it means that we had a measurement get // inserted into the filter history, and our state estimate was updated after it was already // published. As of ROS Noetic, TF2 will issue warnings whenever this occurs, so we make this @@ -2455,7 +2478,7 @@ void UkfWrapper::periodicUpdate() // Publish the acceleration if desired and filter is initialized auto filtered_acceleration = std::make_unique(); - if (!corrected_data && publish_acceleration_ && + if (filter_state_valid && !corrected_data && publish_acceleration_ && getFilteredAccelMessage(filtered_acceleration.get())) { accel_pub_->publish(std::move(filtered_acceleration)); From d2c216b283f0afbcb417fbc72ddab417844493cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 19 Aug 2026 11:00:14 +0200 Subject: [PATCH 2/2] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- .../src/easynav_fusion_localizer/ukf_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 0b9fe9aa..dc6f1f48 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -2359,7 +2359,7 @@ void UkfWrapper::periodicUpdate() RCLCPP_ERROR( parent_node_->get_logger(), "Critical Error, NaNs were detected in the output state of the filter. " - "This was likely due to poorly coniditioned process, noise, or sensor " + "This was likely due to poorly conditioned process, noise, or sensor " "covariances. Resetting the filter."); reset(); filter_state_valid = false;