From 99d40965f2867123dddfc123b3ec27425a67b292 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 3 Jan 2018 11:55:57 +0100
Subject: [PATCH] add more verbose warnings

... as suggested in #1182
---
 src/rviz/default_plugin/covariance_visual.cpp         |  6 ++++--
 .../interactive_markers/interactive_marker.cpp        |  6 ++++--
 src/rviz/default_plugin/pose_array_display.cpp        | 11 +++++++----
 3 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/rviz/default_plugin/covariance_visual.cpp b/src/rviz/default_plugin/covariance_visual.cpp
index 10f3857215..734aa277ba 100644
--- a/src/rviz/default_plugin/covariance_visual.cpp
+++ b/src/rviz/default_plugin/covariance_visual.cpp
@@ -301,8 +301,10 @@ void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& p
 
   // store orientation in Ogre structure
   Ogre::Quaternion ori;
-  if (!normalizeQuaternion(pose.pose.orientation, ori))
-    ROS_WARN("Invalid quaternion (zero length)");
+  if (!normalizeQuaternion(pose.pose.orientation, ori)) {
+    ROS_WARN_ONCE_NAMED("quaternions", "Covariance contains invalid quaternion (zero length)");
+    ROS_DEBUG_NAMED("quaternions", "Covariance contains invalid quaternion (zero length)");
+  }
 
   // Set the orientation of the fixed node. Since this node is attached to the root node, it's orientation will be the
   // inverse of pose's orientation.
diff --git a/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp b/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp
index 21a07dee9c..c5d1ea159d 100644
--- a/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp
+++ b/src/rviz/default_plugin/interactive_markers/interactive_marker.cpp
@@ -120,8 +120,10 @@ bool InteractiveMarker::processMessage( const visualization_msgs::InteractiveMar
       message.pose.position.y,
       message.pose.position.z );
 
-  if (!normalizeQuaternion(message.pose.orientation, orientation_))
-     ROS_WARN("Invalid quaternion (zero length)");
+  if (!normalizeQuaternion(message.pose.orientation, orientation_)) {
+    ROS_WARN_ONCE_NAMED("quaternions", "Interactive marker '%s' contains invalid quaternion (zero length)", name_.c_str());
+    ROS_DEBUG_NAMED("quaternions", "Interactive marker '%s' contains invalid quaternion (zero length)", name_.c_str());
+  }
 
   pose_changed_ =false;
   time_since_last_feedback_ = 0;
diff --git a/src/rviz/default_plugin/pose_array_display.cpp b/src/rviz/default_plugin/pose_array_display.cpp
index bf1fea5c40..ba87eea9b0 100644
--- a/src/rviz/default_plugin/pose_array_display.cpp
+++ b/src/rviz/default_plugin/pose_array_display.cpp
@@ -63,11 +63,13 @@ namespace
     return Ogre::Vector3( point.x, point.y, point.z );
   }
 
-  Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion )
+  Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion, const char* topic )
   {
     Ogre::Quaternion q;
-    if (!normalizeQuaternion(quaternion, q))
-      ROS_WARN("Invalid quaternion (zero length)");
+    if (!normalizeQuaternion(quaternion, q)) {
+      ROS_WARN_ONCE_NAMED("quaternions", "Invalid quaternion (zero length) in PoseArray msg received on topic: %s", topic);
+      ROS_DEBUG_NAMED("quaternions", "Invalid quaternion (zero length) in PoseArray msg received on topic: %s", topic);
+    }
     return q;
   }
 }
@@ -151,11 +153,12 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
     return;
   }
 
+  const char* topic = topic_property_->getTopicStd().c_str();
   poses_.resize( msg->poses.size() );
   for (std::size_t i = 0; i < msg->poses.size(); ++i)
   {
     poses_[i].position = vectorRosToOgre( msg->poses[i].position );
-    poses_[i].orientation = quaternionRosToOgre( msg->poses[i].orientation );
+    poses_[i].orientation = quaternionRosToOgre( msg->poses[i].orientation, topic );
   }
 
   updateDisplay();