Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Warn on unnormalised quaternions instead of rejecting them #1182

Merged
merged 3 commits into from
Jan 4, 2018
Merged

Warn on unnormalised quaternions instead of rejecting them #1182

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
965e5f3
Allow null quaternions to pass validateQuaternions check
dhood Jan 3, 2018
e118da0
Add debug logging of unnormalized quat's norm
dhood Jan 3, 2018
1328320
Warn once if display has invalid quaternion; debug log the repeated
dhood Jan 3, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions src/rviz/default_plugin/interactive_marker_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,9 +232,12 @@ void InteractiveMarkerDisplay::updateMarkers(

if( !validateQuaternions( marker ))
{
setStatusStd( StatusProperty::Error, marker.name,
"Marker contains invalid quaternions (length not equal to 1)!" );
continue;
ROS_WARN_ONCE_NAMED( "quaternions", "Interactive marker '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
marker.name.c_str() );
ROS_DEBUG_NAMED( "quaternions", "Interactive marker '%s' contains unnormalized quaternions.",
marker.name.c_str() );
}
ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() );

Expand Down
8 changes: 6 additions & 2 deletions src/rviz/default_plugin/map_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -658,8 +658,12 @@ void MapDisplay::showMap()

if( !validateQuaternions( current_map_.info.origin ))
{
setStatus( StatusProperty::Error, "Map", "Message contained invalid quaternions (length not equal to 1)!" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "Map received on topic '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
topic_property_->getTopicStd().c_str() );
ROS_DEBUG_NAMED( "quaternions", "Map received on topic '%s' contains unnormalized quaternions.",
topic_property_->getTopicStd().c_str() );
}

if( current_map_.info.width * current_map_.info.height == 0 )
Expand Down
9 changes: 6 additions & 3 deletions src/rviz/default_plugin/marker_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -303,9 +303,12 @@ void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr&

if( !validateQuaternions( message->pose ))
{
setMarkerStatus( MarkerID( message->ns, message->id ), StatusProperty::Error,
"Contains invalid quaternions (length not equal to 1)!" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "Marker '%s/%d' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
message->ns.c_str(), message->id );
ROS_DEBUG_NAMED( "quaternions", "Marker '%s/%d' contains unnormalized quaternions.",
message->ns.c_str(), message->id );
}

switch ( message->action )
Expand Down
8 changes: 6 additions & 2 deletions src/rviz/default_plugin/odometry_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,8 +266,12 @@ void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& messag

if( !validateQuaternions( message->pose.pose ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained unnormalized quaternion (squares of values don't add to 1)" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "Odometry '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
qPrintable( getName() ) );
ROS_DEBUG_NAMED( "quaternions", "Odometry '%s' contains unnormalized quaternions.",
qPrintable( getName() ) );
}

if( last_used_message_ )
Expand Down
8 changes: 6 additions & 2 deletions src/rviz/default_plugin/path_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -426,8 +426,12 @@ void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )

if( !validateQuaternions( msg->poses ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "Path '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
qPrintable( getName() ) );
ROS_DEBUG_NAMED( "quaternions", "Path '%s' contains unnormalized quaternions.",
qPrintable( getName() ) );
}

// Lookup transform into fixed frame
Expand Down
8 changes: 6 additions & 2 deletions src/rviz/default_plugin/pose_array_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,8 +144,12 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&

if( !validateQuaternions( msg->poses ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "PoseArray '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
qPrintable( getName() ) );
ROS_DEBUG_NAMED( "quaternions", "PoseArray '%s' contains unnormalized quaternions.",
qPrintable( getName() ) );
}

if( !setTransform( msg->header ) )
Expand Down
8 changes: 6 additions & 2 deletions src/rviz/default_plugin/pose_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -264,8 +264,12 @@ void PoseDisplay::processMessage( const geometry_msgs::PoseStamped::ConstPtr& me

if( !validateQuaternions( *message ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "Pose '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
qPrintable( getName() ) );
ROS_DEBUG_NAMED( "quaternions", "Pose '%s' contains unnormalized quaternions.",
qPrintable( getName() ) );
}

Ogre::Vector3 position;
Expand Down
8 changes: 6 additions & 2 deletions src/rviz/default_plugin/pose_with_covariance_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -308,8 +308,12 @@ void PoseWithCovarianceDisplay::processMessage( const geometry_msgs::PoseWithCov

if( !validateQuaternions( message->pose.pose ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid quaternions (length not equal to 1)" );
return;
ROS_WARN_ONCE_NAMED( "quaternions", "PoseWithCovariance '%s' contains unnormalized quaternions. "
"This warning will only be output once but may be true for others; "
"enable DEBUG messages for ros.rviz.quaternions to see more details.",
qPrintable( getName() ) );
ROS_DEBUG_NAMED( "quaternions", "PoseWithCovariance '%s' contains unnormalized quaternions.",
qPrintable( getName() ) );
}

Ogre::Vector3 position;
Expand Down
23 changes: 21 additions & 2 deletions src/rviz/validate_quaternions.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@

#include <geometry_msgs/PoseStamped.h>
#include <OgreQuaternion.h>
#include <ros/ros.h>
#include <tf/LinearMath/Quaternion.h>

#include <boost/array.hpp>
Expand All @@ -41,12 +42,30 @@ 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;
if ( 0.0f == x && 0.0f == y && 0.0f == z && 0.0f == w )
{
// Allow null quaternions to pass because they are common in uninitialized ROS messages.
return true;
}
float norm2 = w * w + x * x + y * y + z * z;
bool is_normalized = std::abs( norm2 - 1.0f ) < 10e-3f;
ROS_DEBUG_COND_NAMED( !is_normalized, "quaternions", "Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. "
"Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
return is_normalized;
}

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;
if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w )
{
// Allow null quaternions to pass because they are common in uninitialized ROS messages.
return true;
}
double norm2 = w * w + x * x + y * y + z * z;
bool is_normalized = std::abs( norm2 - 1.0 ) < 10e-3;
ROS_DEBUG_COND_NAMED( !is_normalized, "quaternions", "Quaternion [x: %.3f, y: %.3f, z: %.3f, w: %.3f] not normalized. "
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the reason for logging here as opposed to the callers is because it can be a nested quaternion that causes a message to be invalid, so it's not so straightforward to have the caller print the norm. Still, it seems a bit awkward to have logging here so we could just remove it altogether without too much impact (trade off of less info)

"Magnitude: %.3f", x, y, z, w, std::sqrt(norm2) );
return is_normalized;
}

inline bool validateQuaternions( Ogre::Quaternion quaternion )
Expand Down