Check for odometry quaternion normalization before display (#1139)

ros-visualization · Aug 17, 2017 · 73ad3e1 · 73ad3e1 · zhuyiif · Dec 29, 2017
1 parent b01e255
commit 73ad3e1
Showing 1 changed file with 15 additions and 0 deletions.
diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp
@@ -253,6 +253,15 @@ bool validateFloats(const nav_msgs::Odometry& msg)
   return valid;
 }
 
+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 )
 {
   typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr;
@@ -263,6 +272,12 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag
     return;
   }
 
+  if( !validateQuaternion( *message ))
+  {
+    setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)");
+    return;
+  }
+
   if( last_used_message_ )
   {
     Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);