diff --git a/src/rviz/default_plugin/interactive_marker_display.cpp b/src/rviz/default_plugin/interactive_marker_display.cpp index cd94cea7da..ca5ca25c17 100644 --- a/src/rviz/default_plugin/interactive_marker_display.cpp +++ b/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() ) diff --git a/src/rviz/default_plugin/map_display.cpp b/src/rviz/default_plugin/map_display.cpp index f0ea4adf1b..6e883886c9 100644 --- a/src/rviz/default_plugin/map_display.cpp +++ b/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; diff --git a/src/rviz/default_plugin/marker_display.cpp b/src/rviz/default_plugin/marker_display.cpp index 09987965f0..c4ca0deec3 100644 --- a/src/rviz/default_plugin/marker_display.cpp +++ b/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; } diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index 31b22af74b..d6feb72bf2 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/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 #include @@ -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; } diff --git a/src/rviz/default_plugin/path_display.cpp b/src/rviz/default_plugin/path_display.cpp index 89fa2591d7..9722c77374 100644 --- a/src/rviz/default_plugin/path_display.cpp +++ b/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; diff --git a/src/rviz/default_plugin/pose_array_display.cpp b/src/rviz/default_plugin/pose_array_display.cpp index ddeaa84e03..95e82abf2c 100644 --- a/src/rviz/default_plugin/pose_array_display.cpp +++ b/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" ); diff --git a/src/rviz/default_plugin/pose_display.cpp b/src/rviz/default_plugin/pose_display.cpp index 46b2c92a48..82c9376b57 100644 --- a/src/rviz/default_plugin/pose_display.cpp +++ b/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 )) diff --git a/src/rviz/default_plugin/pose_with_covariance_display.cpp b/src/rviz/default_plugin/pose_with_covariance_display.cpp index c8ab2ff615..42dae0f649 100644 --- a/src/rviz/default_plugin/pose_with_covariance_display.cpp +++ b/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 )) diff --git a/src/rviz/validate_quaternions.h b/src/rviz/validate_quaternions.h new file mode 100644 index 0000000000..005b273556 --- /dev/null +++ b/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 +#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