add more verbose warnings

... as suggested in ros-visualization#1182
rhaschke · Jan 3, 2018 · 99d4096 · 99d4096
1 parent 76e3a52
commit 99d4096
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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();