From 93f304a703344b4d1cc191da3707a1e870e1c5e6 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 28 Aug 2024 16:40:28 +0200 Subject: [PATCH 1/2] Fixed invalid TF check --- fixposition_driver_lib/src/messages/fpa/tf.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/fixposition_driver_lib/src/messages/fpa/tf.cpp b/fixposition_driver_lib/src/messages/fpa/tf.cpp index 7e0d46c..16e7337 100644 --- a/fixposition_driver_lib/src/messages/fpa/tf.cpp +++ b/fixposition_driver_lib/src/messages/fpa/tf.cpp @@ -68,7 +68,11 @@ void FP_TF::ConvertFromTokens(const std::vector& tokens) { tokens.at(orientation_y_idx), tokens.at(orientation_z_idx)); // Check if TF is valid - if (!tf.rotation.vec().isZero() && !(tf.rotation.w() == 0)) valid_tf = true; + if (tf.rotation.w() == 0 && tf.rotation.vec().isZero()) { + valid_tf = false; + } else { + valid_tf = true; + } } } // namespace fixposition From 3504c1918107cd15af3b61881b569aa1a4f58661 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 28 Aug 2024 17:10:46 +0200 Subject: [PATCH 2/2] Improved syntaxis --- fixposition_driver_lib/src/messages/fpa/tf.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/fixposition_driver_lib/src/messages/fpa/tf.cpp b/fixposition_driver_lib/src/messages/fpa/tf.cpp index 16e7337..d0bb1e2 100644 --- a/fixposition_driver_lib/src/messages/fpa/tf.cpp +++ b/fixposition_driver_lib/src/messages/fpa/tf.cpp @@ -68,11 +68,7 @@ void FP_TF::ConvertFromTokens(const std::vector& tokens) { tokens.at(orientation_y_idx), tokens.at(orientation_z_idx)); // Check if TF is valid - if (tf.rotation.w() == 0 && tf.rotation.vec().isZero()) { - valid_tf = false; - } else { - valid_tf = true; - } + valid_tf = !(tf.rotation.w() == 0 && tf.rotation.vec().isZero()); } } // namespace fixposition