From a10954213cb7e3a74414d2925b823d1a8ca8ebc7 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 28 Nov 2023 12:11:00 -0700 Subject: [PATCH] Added the warning messages back into the estimator. --- rosplane/src/estimator_example.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 2e2ecea..5df6a20 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -338,7 +338,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o } bool problem = false; -// int prob_index; + int prob_index; for (int i = 0; i < 7; i++) { if (!std::isfinite(xhat_p_(i))) @@ -346,7 +346,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o if (!problem) { problem = true; -// prob_index = i; + prob_index = i; } switch (i) { @@ -380,7 +380,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o } if (problem) { -// RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index); TODO come back and track down why pn is going infinite. + RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index); } if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f)) { @@ -396,9 +396,6 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o float wehat = xhat_p_(5); float psihat = xhat_p_(6); - -// RCLCPP_INFO_STREAM(this->get_logger(), "chihat: " << chihat); - output.pn = pnhat; output.pe = pehat; output.h = hhat;