diff --git a/src/rviz/default_plugin/interactive_marker_display.cpp b/src/rviz/default_plugin/interactive_marker_display.cpp index ca5ca25c17..cd94cea7da 100644 --- 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 index 6e883886c9..f0ea4adf1b 100644 --- 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 index c4ca0deec3..09987965f0 100644 --- 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 index d6feb72bf2..31b22af74b 100644 --- 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 #include @@ -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 index 9722c77374..89fa2591d7 100644 --- 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 index 95e82abf2c..ddeaa84e03 100644 --- 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 index 82c9376b57..46b2c92a48 100644 --- 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 index 42dae0f649..c8ab2ff615 100644 --- 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 deleted file mode 100644 index 005b273556..0000000000 --- a/src/rviz/validate_quaternions.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * 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 -#include -#include - -#include - -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 -inline bool validateQuaternions( const std::vector &vec ) -{ - typedef std::vector 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 -inline bool validateQuaternions( const boost::array &arr ) -{ - typedef boost::array 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