bugfix (cra-ros-pkg#809): check if covariance values are specified or…

… not Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
daisukes · May 3, 2023 · ed9d0c4 · ed9d0c4
1 parent 284c2ec
commit ed9d0c4
Showing 1 changed file with 10 additions and 6 deletions.
diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
@@ -1857,16 +1857,20 @@ void RosFilter<T>::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<typename T>