Normalize quaternions from msgs before Ogre use

src/rviz/default_plugin/covariance_visual.cpp

@@ -301,8 +301,7 @@ void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& p
 
   // store orientation in Ogre structure
   Ogre::Quaternion ori;
-  if (!normalizeQuaternion(pose.pose.orientation, ori))
-    ROS_WARN("invalid quaternion (zero length)");
+  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.

src/rviz/default_plugin/interactive_marker_display.cpp

@@ -34,6 +34,7 @@
 #include "rviz/properties/ros_topic_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 #include "rviz/display_context.h"
 
 #include "rviz/default_plugin/interactive_marker_display.h"
@@ -61,6 +62,20 @@ bool validateFloats(const visualization_msgs::InteractiveMarker& msg)
   }
   return valid;
 }
+
+bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
+{
+  if ( !validateQuaternions( marker.pose.orientation )) return false;
+  for ( int c = 0; c < marker.controls.size(); ++c )
+  {
+    if ( !validateQuaternions( marker.controls[c].orientation )) return false;
+    for ( int m = 0; m < marker.controls[c].markers.size(); ++m )
+    {
+      if ( !validateQuaternions( marker.controls[c].markers[m].pose.orientation )) return false;
+    }
+  }
+  return true;
+}
 /////////////
 
 
@@ -214,6 +229,16 @@ void InteractiveMarkerDisplay::updateMarkers(
       //setStatusStd( StatusProperty::Error, "General", "Marker " + marker.name + " contains invalid floats!" );
       continue;
     }
+
+    if( !validateQuaternions( marker ))
+    {
+      ROS_WARN_ONCE_NAMED( "quaternions", "Interactive marker '%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.",
+                           marker.name.c_str() );
+      ROS_DEBUG_NAMED( "quaternions", "Interactive marker '%s' contains unnormalized quaternions.",
+                       marker.name.c_str() );
+    }
     ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() );
 
     std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker.name );
@@ -274,6 +299,13 @@ void InteractiveMarkerDisplay::updatePoses(
       return;
     }
 
+    if( !validateQuaternions( marker_pose.pose ))
+    {
+      setStatusStd( StatusProperty::Error, marker_pose.name,
+                    "Pose message contains invalid quaternions (length not equal to 1)!" );
+      return;
+    }
+
     std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker_pose.name );
 
     if ( int_marker_entry != im_map.end() )

src/rviz/default_plugin/interactive_markers/interactive_marker.cpp

@@ -120,8 +120,7 @@ bool InteractiveMarker::processMessage( const visualization_msgs::InteractiveMar
       message.pose.position.y,
       message.pose.position.z );
 
-  if (!normalizeQuaternion(message.pose.orientation, orientation_))
-     ROS_WARN("invalid quaternion (zero length)");
+  normalizeQuaternion(message.pose.orientation, orientation_);
 
   pose_changed_ =false;
   time_since_last_feedback_ = 0;

src/rviz/default_plugin/map_display.cpp

@@ -656,11 +656,14 @@ void MapDisplay::showMap()
     return;
   }
 
-  Ogre::Quaternion orientation;
-  if( !normalizeQuaternion(current_map_.info.origin.orientation, orientation))
+  if( !validateQuaternions( current_map_.info.origin ))
   {
-    setStatus( StatusProperty::Error, "Map", "Message has invalid quaternion (length close to zero)!" );
-    return;
+    ROS_WARN_ONCE_NAMED( "quaternions", "Map 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.",
+                         topic_property_->getTopicStd().c_str() );
+    ROS_DEBUG_NAMED( "quaternions", "Map received on topic '%s' contains unnormalized quaternions.", 
+                     topic_property_->getTopicStd().c_str() );
   }
 
   if( current_map_.info.width * current_map_.info.height == 0 )
@@ -693,6 +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;
+  normalizeQuaternion( current_map_.info.origin.orientation, orientation );
+
   frame_ = current_map_.header.frame_id;
   if (frame_.empty())
   {

src/rviz/default_plugin/marker_display.cpp

@@ -49,6 +49,7 @@
 #include "rviz/properties/ros_topic_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 
 #include "rviz/default_plugin/marker_display.h"
 
@@ -300,6 +301,16 @@ void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr&
     return;
   }
 
+  if( !validateQuaternions( message->pose ))
+  {
+    ROS_WARN_ONCE_NAMED( "quaternions", "Marker '%s/%d' 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.",
+                         message->ns.c_str(), message->id );
+    ROS_DEBUG_NAMED( "quaternions", "Marker '%s/%d' contains unnormalized quaternions.", 
+                     message->ns.c_str(), message->id );
+  }
+
   switch ( message->action )
   {
   case visualization_msgs::Marker::ADD:

src/rviz/default_plugin/odometry_display.cpp

@@ -34,6 +34,7 @@
 #include "rviz/properties/float_property.h"
 #include "rviz/properties/int_property.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 
 #include <OgreSceneManager.h>
 #include <OgreSceneNode.h>
@@ -263,6 +264,16 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag
     return;
   }
 
+  if( !validateQuaternions( message->pose.pose ))
+  {
+    ROS_WARN_ONCE_NAMED( "quaternions", "Odometry '%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", "Odometry '%s' contains unnormalized quaternions.", 
+                     qPrintable( getName() ) );
+  }
+
   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);

src/rviz/default_plugin/path_display.cpp

@@ -45,6 +45,7 @@
 #include "rviz/properties/int_property.h"
 #include "rviz/properties/vector_property.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 
 #include "rviz/ogre_helpers/billboard_line.h"
 #include "rviz/default_plugin/path_display.h"
@@ -423,6 +424,16 @@ void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
     return;
   }
 
+  if( !validateQuaternions( msg->poses ))
+  {
+    ROS_WARN_ONCE_NAMED( "quaternions", "Path '%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", "Path '%s' contains unnormalized quaternions.", 
+                     qPrintable( getName() ) );
+  }
+
   // Lookup transform into fixed frame
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;

src/rviz/default_plugin/pose_array_display.cpp

@@ -66,8 +66,7 @@ namespace
   Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion )
   {
     Ogre::Quaternion q;
-    if (!normalizeQuaternion(quaternion, q))
-      ROS_WARN("invalid quaternion (zero length)");
+    normalizeQuaternion( quaternion, q );
     return q;
   }
 }
@@ -145,6 +144,16 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
     return;
   }
 
+  if( !validateQuaternions( msg->poses ))
+  {
+    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.",
+                         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 ) )
   {
     setStatus( StatusProperty::Error, "Topic", "Failed to look up transform" );

src/rviz/default_plugin/pose_display.cpp

@@ -43,6 +43,7 @@
 #include "rviz/properties/vector_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 
 #include "rviz/default_plugin/pose_display.h"
 
@@ -261,6 +262,16 @@ void PoseDisplay::processMessage( const geometry_msgs::PoseStamped::ConstPtr& me
     return;
   }
 
+  if( !validateQuaternions( *message ))
+  {
+    ROS_WARN_ONCE_NAMED( "quaternions", "Pose '%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", "Pose '%s' contains unnormalized quaternions.",
+                     qPrintable( getName() ) );
+  }
+
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if( !context_->getFrameManager()->transform( message->header, message->pose, position, orientation ))

src/rviz/default_plugin/pose_with_covariance_display.cpp

@@ -44,6 +44,7 @@
 #include "rviz/properties/vector_property.h"
 #include "rviz/selection/selection_manager.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 
 #include "pose_with_covariance_display.h"
 #include "covariance_visual.h"
@@ -305,6 +306,16 @@ void PoseWithCovarianceDisplay::processMessage( const geometry_msgs::PoseWithCov
     return;
   }
 
+  if( !validateQuaternions( message->pose.pose ))
+  {
+    ROS_WARN_ONCE_NAMED( "quaternions", "PoseWithCovariance '%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", "PoseWithCovariance '%s' contains unnormalized quaternions.", 
+                     qPrintable( getName() ) );
+  }
+
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation ))