Normalize quaternions from msgs before Ogre use

src/rviz/default_plugin/covariance_visual.cpp

@@ -30,6 +30,7 @@
 #include "covariance_visual.h"
 
 #include "rviz/ogre_helpers/shape.h"
+#include "rviz/validate_quaternions.h"
 
 #include <OgreSceneManager.h>
 #include <OgreSceneNode.h>
@@ -299,7 +300,9 @@ void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& p
   updateOrientationVisibility();
 
   // store orientation in Ogre structure
-  Ogre::Quaternion ori(pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z);
+  Ogre::Quaternion ori;
+  normalizeQuaternion(pose.pose.orientation, ori);
+
   // Set the orientation of the fixed node. Since this node is attached to the root node, it's orientation will be the
   // inverse of pose's orientation.
   fixed_orientation_node_->setOrientation(ori.Inverse());

src/rviz/default_plugin/interactive_markers/interactive_marker.cpp

@@ -48,6 +48,7 @@
 #include "rviz/frame_manager.h"
 #include "rviz/render_panel.h"
 #include "rviz/geometry.h"
+#include "rviz/validate_quaternions.h"
 
 #include "rviz/default_plugin/interactive_markers/integer_action.h"
 #include "rviz/default_plugin/interactive_markers/interactive_marker.h"
@@ -119,11 +120,7 @@ bool InteractiveMarker::processMessage( const visualization_msgs::InteractiveMar
       message.pose.position.y,
       message.pose.position.z );
 
-  orientation_ = Ogre::Quaternion(
-      message.pose.orientation.w,
-      message.pose.orientation.x,
-      message.pose.orientation.y,
-      message.pose.orientation.z );
+  normalizeQuaternion(message.pose.orientation, orientation_);
 
   pose_changed_ =false;
   time_since_last_feedback_ = 0;

src/rviz/default_plugin/map_display.cpp

@@ -696,10 +696,9 @@ void MapDisplay::showMap()
   Ogre::Vector3 position( current_map_.info.origin.position.x,
                           current_map_.info.origin.position.y,
                           current_map_.info.origin.position.z );
-  Ogre::Quaternion orientation( current_map_.info.origin.orientation.w,
-                                current_map_.info.origin.orientation.x,
-                                current_map_.info.origin.orientation.y,
-                                current_map_.info.origin.orientation.z );
+  Ogre::Quaternion orientation;
+  normalizeQuaternion( current_map_.info.origin.orientation, orientation );
+
   frame_ = current_map_.header.frame_id;
   if (frame_.empty())
   {

src/rviz/default_plugin/pose_array_display.cpp

@@ -65,7 +65,9 @@ namespace
 
   Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion )
   {
-    return Ogre::Quaternion( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
+    Ogre::Quaternion q;
+    normalizeQuaternion( quaternion, q );
+    return q;
   }
 }
 
@@ -144,12 +146,12 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
 
   if( !validateQuaternions( msg->poses ))
   {
-    ROS_WARN_ONCE_NAMED( "quaternions", "PoseArray '%s' contains unnormalized quaternions. "
+    ROS_WARN_ONCE_NAMED( "quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions. "
                          "This warning will only be output once but may be true for others; "
                          "enable DEBUG messages for ros.rviz.quaternions to see more details.",
-                         qPrintable( getName() ) );
-    ROS_DEBUG_NAMED( "quaternions", "PoseArray '%s' contains unnormalized quaternions.", 
-                     qPrintable( getName() ) );
+                         topic_property_->getTopicStd().c_str() );
+    ROS_DEBUG_NAMED( "quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions.", 
+                     topic_property_->getTopicStd().c_str() );
   }
 
   if( !setTransform( msg->header ) )

src/rviz/validate_quaternions.h

@@ -40,40 +40,89 @@
 namespace rviz
 {
 
+// Tolerance for acceptable quaternion normalization (same as in tf)
+static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3;
+
+inline float quaternionNorm2( float w, float x, float y, float z )
+{
+  return w * w + x * x + y * y + z * z;
+}
+
 inline bool validateQuaternions( float w, float x, float y, float z )
 {
   if ( 0.0f == x && 0.0f == y && 0.0f == z && 0.0f == w )
   {
     // Allow null quaternions to pass because they are common in uninitialized ROS messages.
     return true;
   }
-  float norm2 = w * w + x * x + y * y + z * z; 
-  bool is_normalized = std::abs( norm2 - 1.0f ) < 10e-3f;
+  float norm2 = quaternionNorm2( w, x, y, z ); 
+  bool is_normalized = std::abs( norm2 - 1.0f ) < QUATERNION_NORMALIZATION_TOLERANCE;
   ROS_DEBUG_COND_NAMED( !is_normalized, "quaternions", "Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. "
                         "Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
   return is_normalized;
 }
 
+inline float normalizeQuaternion( float& w, float& x, float& y, float& z )
+{
+  if ( 0.0f == x && 0.0f == y && 0.0f == z && 0.0f == w )
+  {
+    w = 1.0f;
+    return 0.0f;
+  }
+  float norm2 = quaternionNorm2( w, x, y, z );
+  norm2 = std::sqrt( norm2 );
+  float invnorm = 1.0f / norm2;
+  w *= invnorm;
+  x *= invnorm;
+  y *= invnorm;
+  z *= invnorm;
+  return norm2;
+}
+
+
+inline double quaternionNorm2( double w, double x, double y, double z )
+{
+  return w * w + x * x + y * y + z * z;
+}
+
 inline bool validateQuaternions( double w, double x, double y, double z )
 {
   if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
   {
     // Allow null quaternions to pass because they are common in uninitialized ROS messages.
     return true;
   }
-  double norm2 = w * w + x * x + y * y + z * z; 
-  bool is_normalized = std::abs( norm2 - 1.0 ) < 10e-3;
+  double norm2 = quaternionNorm2( w, x, y, z ); 
+  bool is_normalized = std::abs( norm2 - 1.0 ) < QUATERNION_NORMALIZATION_TOLERANCE;
   ROS_DEBUG_COND_NAMED( !is_normalized, "quaternions", "Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. "
                         "Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
   return is_normalized;
 }
 
-inline bool validateQuaternions( Ogre::Quaternion quaternion )
+inline double normalizeQuaternion( double& w, double& x, double& y, double& z )
+{
+  if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
+  {
+    w = 1.0;
+    return 0.0;
+  }
+  double norm2 = quaternionNorm2( w, x, y, z );
+  norm2 = std::sqrt( norm2 );
+  double invnorm = 1.0 / norm2;
+  w *= invnorm;
+  x *= invnorm;
+  y *= invnorm;
+  z *= invnorm;
+  return norm2;
+}
+
+
+inline bool validateQuaternions( const Ogre::Quaternion& quaternion )
 {
   return validateQuaternions( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
 }
 
-inline bool validateQuaternions( tf::Quaternion quaternion )
+inline bool validateQuaternions( const tf::Quaternion& quaternion )
 {
   return validateQuaternions( quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
 }
@@ -93,6 +142,20 @@ inline bool validateQuaternions( const geometry_msgs::PoseStamped &msg )
   return validateQuaternions( msg.pose );
 }
 
+inline double normalizeQuaternion( Ogre::Quaternion& quaternion )
+{
+  return normalizeQuaternion( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
+}
+
+inline double normalizeQuaternion( const geometry_msgs::Quaternion &msg, Ogre::Quaternion &q )
+{
+  q.w = msg.w;
+  q.x = msg.x;
+  q.y = msg.y;
+  q.z = msg.z;
+  return normalizeQuaternion( q );
+}
+
 template<typename T>
 inline bool validateQuaternions( const std::vector<T> &vec )
 {
@@ -126,6 +189,7 @@ inline bool validateQuaternions( const boost::array<T, N> &arr )
 
   return true;
 }
+
 } // namespace rviz
 
 #endif // RVIZ_VALIDATE_QUATERNIONS_H