Warn on unnormalised quaternions instead of rejecting them #1182
Conversation
| } | ||
| 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. " |
There was a problem hiding this comment.
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)
The covariance display is not the only one, directly using quaternions from ROS msg as an Ogre orientation. As you can see in 3dc2e74, there are several other locations that might potentially cause issues: @dhood All these are nicely handled by #1179 and I don't really understand why you don't consider #1179 as a basis but instead file new similar PRs (#1180, #1182). If you like to have some modifications applied to #1179 you are welcome to do so. |
... as suggested in ros-visualization#1182
|
This is what the original PR #1167 should have been. |
We decided that no uninitialised quaternions will give a warning (https://github.com/ros-visualization/rviz/pull/1182/files#diff-0eefc6614225475a9ff350807d529a19R61), so this will only be the case if they have an unnormalised quaternion in the DELETE msg |
* Revert "Added checks for invalid quaternions. (#1167)" This reverts commit b329145. * normalize invalid quaternions instead of rejecting them Normalization of quaternions usually is done by rviz::FrameManager::transform() which transforms a ROS pose into an Ogre position + orientation. Only in some rare cases, a ROS quaternion was directly used as an Ogre::Quaternion, which then requires handling of null quaternions (as they arise from uninitialized ROS pose msgs). * addressed Dave's comments * add more verbose warnings ... as suggested in #1182 * Revert "Revert "Added checks for invalid quaternions. (#1167)"" This reverts commit 42a4416. * Minimise diff * Warning will already be output for invalid quats when msg validated * Return pre-normalised length from normalizeQuat Don't mix logic for what is considered a valid quat into this function; matches Ogre * Use validateQuaternions for map Logic of what makes a quaternion valid isn't in normalizeQuaternion; also gets the invalid quaternion value logged to console * Remove unnecessary 0 setting * Reduce number of divisions
Since #1180 fixes the only confirmed issue with invalid quaternions, don't reject them anymore.
Null quaternions will be set to identity by
FrameManager::transformwhere appropriate. We do not output a warning for these because they are common in uninitialised ROS messages and are more likely indicative of users just being lazy than an error in the publisher.Non-null, unnormalised quaternions will generate a warning, as it may be from an error in the code of the publisher (see #1179 (comment)). It would be most convenient for this warning to be displayed as a status in the GUI so it can be coupled with the specific offender. However, #1167 has revealed that there are many such cases (particularly markers), and having so many warnings throughout the display may cause more serious warnings to go unnoticed.
Instead, a console warning is generated, with
ONCEsince there's such a high prevalence of unnormalised quaternions around these days (it would be more appropriate to log a warning once for each invalid quaternion but that'd be less straightforward). Users can access info about all offending messages by setting thequaternionssublogger to debug. If it's more appropriate to potentially flood with console warnings then we can remove the ONCE filter; this seemed like a "gentler" approach.