Added check for invalid quaternions.

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,13 @@ void InteractiveMarkerDisplay::updateMarkers(
       //setStatusStd( StatusProperty::Error, "General", "Marker " + marker.name + " contains invalid floats!" );
       continue;
     }
+
+    if( !validateQuaternions( marker ))
+    {
+      setStatusStd( StatusProperty::Error, marker.name,
+                    "Marker contains invalid quaternions (length not equal to 1)!" );
+      continue;
+    }
     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 +296,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/map_display.cpp

@@ -52,6 +52,7 @@
 #include "rviz/properties/ros_topic_property.h"
 #include "rviz/properties/vector_property.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 #include "rviz/display_context.h"
 
 #include "map_display.h"
@@ -655,6 +656,12 @@ void MapDisplay::showMap()
     return;
   }
 
+  if( !validateQuaternions( current_map_.info.origin ))
+  {
+    setStatus( StatusProperty::Error, "Map", "Message contained invalid quaternions (length not equal to 1)!" );
+    return;
+  }
+
   if( current_map_.info.width * current_map_.info.height == 0 )
   {
     std::stringstream ss;

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"
 
@@ -293,9 +294,17 @@ bool validateFloats(const visualization_msgs::Marker& msg)
 
 void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr& message )
 {
-  if (!validateFloats(*message))
+  if ( !validateFloats( *message ))
   {
-    setMarkerStatus(MarkerID(message->ns, message->id), StatusProperty::Error, "Contains invalid floating point values (nans or infs)");
+    setMarkerStatus( MarkerID( message->ns, message->id ), StatusProperty::Error,
+                     "Contains invalid floating point values (nans or infs)" );
+    return;
+  }
+
+  if( !validateQuaternions( message->pose ))
+  {
+    setMarkerStatus( MarkerID( message->ns, message->id ), StatusProperty::Error,
+                     "Contains invalid quaternions (length not equal to 1)!" );
     return;
   }
 

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>
@@ -253,15 +254,6 @@ 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;
@@ -272,9 +264,9 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag
     return;
   }
 
-  if( !validateQuaternion( *message ))
+  if( !validateQuaternions( message->pose.pose ))
   {
-    setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)");
+    setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)" );
     return;
   }
 

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,12 @@ void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
     return;
   }
 
+  if( !validateQuaternions( msg->poses ))
+  {
+    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
+    return;
+  }
+
   // Lookup transform into fixed frame
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;

src/rviz/default_plugin/pose_array_display.cpp

@@ -37,6 +37,7 @@
 #include "rviz/properties/color_property.h"
 #include "rviz/properties/float_property.h"
 #include "rviz/validate_floats.h"
+#include "rviz/validate_quaternions.h"
 #include "rviz/ogre_helpers/arrow.h"
 #include "rviz/ogre_helpers/axes.h"
 
@@ -141,6 +142,12 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
     return;
   }
 
+  if( !validateQuaternions( msg->poses ))
+  {
+    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
+    return;
+  }
+
   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,12 @@ void PoseDisplay::processMessage( const geometry_msgs::PoseStamped::ConstPtr& me
     return;
   }
 
+  if( !validateQuaternions( *message ))
+  {
+    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
+    return;
+  }
+
   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,12 @@ void PoseWithCovarianceDisplay::processMessage( const geometry_msgs::PoseWithCov
     return;
   }
 
+  if( !validateQuaternions( message->pose.pose ))
+  {
+    setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
+    return;
+  }
+
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation ))

src/rviz/validate_quaternions.h

@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2017, Stefan Fabian
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef RVIZ_VALIDATE_QUATERNIONS_H
+#define RVIZ_VALIDATE_QUATERNIONS_H
+
+#include <geometry_msgs/PoseStamped.h>
+#include <OgreQuaternion.h>
+#include <tf/LinearMath/Quaternion.h>
+
+#include <boost/array.hpp>
+
+namespace rviz
+{
+
+inline bool validateQuaternions( float w, float x, float y, float z )
+{
+  return std::abs( w * w + x * x + y * y + z * z - 1.0f ) < 10e-3f;
+}
+
+inline bool validateQuaternions( double w, double x, double y, double z )
+{
+  return std::abs( w * w + x * x + y * y + z * z - 1.0 ) < 10e-3;
+}
+
+inline bool validateQuaternions( Ogre::Quaternion quaternion )
+{
+  return validateQuaternions( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
+}
+
+inline bool validateQuaternions( tf::Quaternion quaternion )
+{
+  return validateQuaternions( quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
+}
+
+inline bool validateQuaternions( const geometry_msgs::Quaternion &msg )
+{
+  return validateQuaternions( msg.w, msg.x, msg.y, msg.z );
+}
+
+inline bool validateQuaternions( const geometry_msgs::Pose &msg )
+{
+  return validateQuaternions( msg.orientation );
+}
+
+inline bool validateQuaternions( const geometry_msgs::PoseStamped &msg )
+{
+  return validateQuaternions( msg.pose );
+}
+
+template<typename T>
+inline bool validateQuaternions( const std::vector<T> &vec )
+{
+  typedef std::vector<T> VecType;
+  typename VecType::const_iterator it = vec.begin();
+  typename VecType::const_iterator end = vec.end();
+  for ( ; it != end; ++it )
+  {
+    if ( !validateQuaternions( *it ))
+    {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+template<typename T, size_t N>
+inline bool validateQuaternions( const boost::array<T, N> &arr )
+{
+  typedef boost::array<T, N> ArrType;
+  typename ArrType::const_iterator it = arr.begin();
+  typename ArrType::const_iterator end = arr.end();
+  for ( ; it != end; ++it )
+  {
+    if ( !validateQuaternions( *it ))
+    {
+      return false;
+    }
+  }
+
+  return true;
+}
+} // namespace rviz
+
+#endif // RVIZ_VALIDATE_QUATERNIONS_H