Check for odometry quaternion normalization before display (#1139)

ros-visualization · Aug 21, 2017 · 0a3f2be · 0a3f2be
1 parent 396b2a3
commit 0a3f2be
Showing 1 changed file with 15 additions and 2 deletions.
diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp
@@ -203,7 +203,16 @@ bool validateFloats(const nav_msgs::Odometry& msg)
   return valid;
 }
 
-void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
+bool validateQuaternion(const nav_msgs::Odometry& msg)
+{
+  bool valid = std::abs((msg.pose.pose.orientation.x * msg.pose.pose.orientation.x
+                       + msg.pose.pose.orientation.y * msg.pose.pose.orientation.y
+                       + msg.pose.pose.orientation.z * msg.pose.pose.orientation.z
+                       + msg.pose.pose.orientation.w * msg.pose.pose.orientation.w) - 1.0f) < 10e-3;
+  return valid;
+}
+
+void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& message )
 {
   ++messages_received_;
 
@@ -213,7 +222,11 @@ void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& messa
     return;
   }
 
-  setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
+  if( !validateQuaternion( *message ))
+  {
+    setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)");
+    return;
+  }
 
   if( last_used_message_ )
   {