Improve message filters error messages (#364)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

tf2_ros/include/tf2_ros/message_filter.h

@@ -80,11 +80,14 @@ enum FilterFailureReason
   /// The message buffer overflowed, and this message was pushed off the back of the queue, but the
   // reason it was unable to be transformed is unknown.
   Unknown,
-  /// The timestamp on the message is more than the cache length earlier than the newest data in
-  // the transform cache
+  /// The timestamp on the message is earlier than all the data in the transform cache
   OutTheBack,
   /// The frame_id on the message is empty
   EmptyFrameID,
+  /// No transform found
+  NoTransformFound,
+  /// Queue size full
+  QueueFull,
   /// Max enum value for iteration, keep it at the end of the enum
   FilterFailureReasonCount,
 };
@@ -95,14 +98,18 @@ static std::string get_filter_failure_reason_string(
   filter_failure_reasons::FilterFailureReason reason)
 {
   switch (reason) {
-    case filter_failure_reasons::Unknown:
-      return "Unknown";
     case filter_failure_reasons::OutTheBack:
-      return "OutTheBack";
+      return
+        "the timestamp on the message is earlier than all the data in the transform cache";
     case filter_failure_reasons::EmptyFrameID:
-      return "EmptyFrameID";
+      return "the frame id of the message is empty";
+    case filter_failure_reasons::NoTransformFound:
+      return "did not find a valid transform, this usually happens at startup ...";
+    case filter_failure_reasons::QueueFull:
+      return "discarding message because the queue is full";
+    case filter_failure_reasons::Unknown:  // fallthrough
     default:
-      return "Invalid Failure Reason";
+      return "unknown";
   }
 }
 
@@ -397,7 +404,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
           (mt::FrameId<M>::value(*front.event.getMessage())).c_str(),
           mt::TimeStamp<M>::value(*front.event.getMessage()).seconds());
 
-        messageDropped(front.event, filter_failure_reasons::Unknown);
+        messageDropped(front.event, filter_failure_reasons::QueueFull);
 
         messages_.pop_front();
       }
@@ -512,10 +519,12 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
     rclcpp::Time stamp = mt::TimeStamp<M>::value(*message);
 
     bool transform_available = true;
+    FilterFailureReason error = filter_failure_reasons::Unknown;
     try {
       future.get();
     } catch (...) {
       transform_available = false;
+      error = filter_failure_reasons::OutTheBack;
     }
 
     if (transform_available) {
@@ -557,7 +566,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
       TF2_ROS_MESSAGEFILTER_DEBUG(
         "Discarding message in frame %s at time %.3f, count now %d",
         frame_id.c_str(), stamp.seconds(), messages_.size());
-      messageDropped(saved_event, filter_failure_reasons::Unknown);
+      messageDropped(saved_event, error);
     }
   }
 

tf2_ros/test/message_filter_test.cpp

@@ -200,22 +200,13 @@ TEST(tf2_ros_message_filter, failure_reason_string_conversion)
 {
   // Sanity test defined messages
   EXPECT_EQ(
-    "Unknown",
+    "unknown",
     tf2_ros::get_filter_failure_reason_string(tf2_ros::filter_failure_reasons::Unknown)
   );
-  EXPECT_EQ(
-    "OutTheBack",
-    tf2_ros::get_filter_failure_reason_string(tf2_ros::filter_failure_reasons::OutTheBack)
-  );
-  EXPECT_EQ(
-    "EmptyFrameID",
-    tf2_ros::get_filter_failure_reason_string(tf2_ros::filter_failure_reasons::EmptyFrameID)
-  );
-
   // Make sure all values have been given a string
-  for (size_t i = 0; i < tf2_ros::filter_failure_reasons::FilterFailureReasonCount; i++) {
+  for (size_t i = 1; i < tf2_ros::filter_failure_reasons::FilterFailureReasonCount; i++) {
     EXPECT_NE(
-      "Invalid Failure Reason",
+      "unknown",
       tf2_ros::get_filter_failure_reason_string(
         tf2_ros::filter_failure_reasons::FilterFailureReason(i))
     );
@@ -224,7 +215,7 @@ TEST(tf2_ros_message_filter, failure_reason_string_conversion)
   // Test that value outside the defined range has been given a string and that count was
   // maintained at the end
   EXPECT_EQ(
-    "Invalid Failure Reason",
+    "unknown",
     tf2_ros::get_filter_failure_reason_string(
       tf2_ros::filter_failure_reasons::FilterFailureReasonCount)
   );