Revert "Added checks for invalid quaternions. (ros-visualization#1167)"

This reverts commit b329145.
rhaschke · Jan 3, 2018 · 42a4416 · 42a4416
1 parent b329145
commit 42a4416
Show file tree

Hide file tree

Showing 9 changed files with 13 additions and 190 deletions.
diff --git a/src/rviz/default_plugin/interactive_marker_display.cpp b/src/rviz/default_plugin/interactive_marker_display.cpp
@@ -34,7 +34,6 @@
 #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"
@@ -62,20 +61,6 @@ 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;
-}
 /////////////
 
 
@@ -229,13 +214,6 @@ 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 );
@@ -296,13 +274,6 @@ 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() )

diff --git a/src/rviz/default_plugin/map_display.cpp b/src/rviz/default_plugin/map_display.cpp
@@ -52,7 +52,6 @@
 #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"
@@ -656,12 +655,6 @@ 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;

diff --git a/src/rviz/default_plugin/marker_display.cpp b/src/rviz/default_plugin/marker_display.cpp
@@ -49,7 +49,6 @@
 #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"
 
@@ -294,17 +293,9 @@ 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)" );
-    return;
-  }
-
-  if( !validateQuaternions( message->pose ))
-  {
-    setMarkerStatus( MarkerID( message->ns, message->id ), StatusProperty::Error,
-                     "Contains invalid quaternions (length not equal to 1)!" );
+    setMarkerStatus(MarkerID(message->ns, message->id), StatusProperty::Error, "Contains invalid floating point values (nans or infs)");
     return;
   }
 

diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp
@@ -34,7 +34,6 @@
 #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>
@@ -254,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;
@@ -264,9 +272,9 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag
     return;
   }
 
-  if( !validateQuaternions( message->pose.pose ))
+  if( !validateQuaternion( *message ))
   {
-    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;
   }
 

diff --git a/src/rviz/default_plugin/path_display.cpp b/src/rviz/default_plugin/path_display.cpp
@@ -45,7 +45,6 @@
 #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"
@@ -424,12 +423,6 @@ 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;

diff --git a/src/rviz/default_plugin/pose_array_display.cpp b/src/rviz/default_plugin/pose_array_display.cpp
@@ -37,7 +37,6 @@
 #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"
 
@@ -142,12 +141,6 @@ 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" );

diff --git a/src/rviz/default_plugin/pose_display.cpp b/src/rviz/default_plugin/pose_display.cpp
@@ -43,7 +43,6 @@
 #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"
 
@@ -262,12 +261,6 @@ 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 ))

diff --git a/src/rviz/default_plugin/pose_with_covariance_display.cpp b/src/rviz/default_plugin/pose_with_covariance_display.cpp
@@ -44,7 +44,6 @@
 #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"
@@ -306,12 +305,6 @@ 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 ))

diff --git a/src/rviz/validate_quaternions.h b/src/rviz/validate_quaternions.h