fix: don't publish twist flu if it has nan values

Greenroom-Robotics · Oct 22, 2024 · 7196f38 · 7196f38
1 parent f188cfc
commit 7196f38
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp
@@ -1121,7 +1121,14 @@ namespace io {
             twist_flu_msg.header = msg.header;
             twist_flu_msg.header.frame_id = settings_->frame_id;
             twist_flu_msg.twist = msg.twist.twist;
-            publish<TwistStampedMsg>("twist_flu", twist_flu_msg);
+
+            if (!(std::isnan(twist_flu_msg.twist.linear.x) ||
+                std::isnan(twist_flu_msg.twist.linear.y) ||
+                std::isnan(twist_flu_msg.twist.linear.z)))
+            {
+                publish<TwistStampedMsg>("twist_flu", twist_flu_msg);
+            }
+
         }   
 
         if (settings_->publish_localization)