odom_estimator: warn on non-unit VelocityMeasurement::direction vector

uf-mil · Apr 19, 2024 · 934d7b4 · 934d7b4
1 parent fe3938d
commit 934d7b4
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/mil_common/gnc/odom_estimator/src/nodelet.cpp b/mil_common/gnc/odom_estimator/src/nodelet.cpp
@@ -281,6 +281,10 @@ class NodeImpl
     for (unsigned int i = 0; i < msg.velocity_measurements.size(); i++)
     {
       mil_msgs::VelocityMeasurement const &vm = msg.velocity_measurements[i];
+      if (std::fabs(xyz2vec(vm.direction).norm() - 1) > 1e-3)
+      {
+        NODELET_ERROR("VelocityMeasurement::direction should always be a unit vector");
+      }
       if (!std::isnan(vm.velocity))
       {
         good.push_back(vm);