From 73ad3e122283b2a8cbd116bda0b2ccf63c3b03e7 Mon Sep 17 00:00:00 2001 From: Simon Harst Date: Thu, 17 Aug 2017 23:13:38 +0200 Subject: [PATCH] Check for odometry quaternion normalization before display (#1139) --- src/rviz/default_plugin/odometry_display.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/rviz/default_plugin/odometry_display.cpp b/src/rviz/default_plugin/odometry_display.cpp index 3c86b85f99..31b22af74b 100644 --- a/src/rviz/default_plugin/odometry_display.cpp +++ b/src/rviz/default_plugin/odometry_display.cpp @@ -253,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; @@ -263,6 +272,12 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag return; } + if( !validateQuaternion( *message )) + { + setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)"); + return; + } + 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);