From ed9d0c4b041d5855169e71cd3bdf4953b38aa578 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 3 May 2023 19:07:48 -0400 Subject: [PATCH] bugfix (#809): check if covariance values are specified or not Signed-off-by: Daisuke Sato --- src/ros_filter.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index fe4b22cc8..b13aa3c0d 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -1857,16 +1857,20 @@ void RosFilter::loadParams() RCLCPP_FATAL_STREAM(get_logger(), error); throw std::invalid_argument(error); } + return true; } + return false; }; - load_covariance("process_noise_covariance", process_noise_covariance_); - RF_DEBUG("Process noise covariance is:\n" << process_noise_covariance_ << "\n"); - filter_.setProcessNoiseCovariance(process_noise_covariance_); + if (load_covariance("process_noise_covariance", process_noise_covariance_)) { + RF_DEBUG("Process noise covariance is:\n" << process_noise_covariance_ << "\n"); + filter_.setProcessNoiseCovariance(process_noise_covariance_); + } - load_covariance("initial_estimate_covariance", initial_estimate_error_covariance_); - RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); - filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); + if (load_covariance("initial_estimate_covariance", initial_estimate_error_covariance_)) { + RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); + filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); + } } template