From f5f54d75d360f64c8adc73096c915ee5279417de Mon Sep 17 00:00:00 2001
From: Kirk MacTavish <kirk.mactavish@mail.utoronto.ca>
Date: Thu, 3 Aug 2017 01:14:53 -0400
Subject: [PATCH] Debugs failing empty timestamp test.

---
 ...amped_topic_statistics_empty_timestamp.cpp | 40 +++++++++++++++----
 1 file changed, 33 insertions(+), 7 deletions(-)

diff --git a/test/test_roscpp/test/src/stamped_topic_statistics_empty_timestamp.cpp b/test/test_roscpp/test/src/stamped_topic_statistics_empty_timestamp.cpp
index 08f1a49733..8d5f4f8daa 100644
--- a/test/test_roscpp/test/src/stamped_topic_statistics_empty_timestamp.cpp
+++ b/test/test_roscpp/test/src/stamped_topic_statistics_empty_timestamp.cpp
@@ -45,18 +45,44 @@ TEST(TopicStatistics, empty_timestamp_crash_check)
   ros::Publisher pub = nh.advertise<test_roscpp::TestWithHeader>("test_with_empty_timestamp", 0);
   ros::Subscriber sub = nh.subscribe("test_with_empty_timestamp", 0, callback);
 
+  ros::Duration delay_to_publish;
+  try {
+    delay_to_publish.fromSec(10.0);
+  } catch (std::runtime_error & e) {
+    ROS_FATAL_STREAM("It was in the duration: " << e.what()); throw;
+  }
+    
   ros::Time start = ros::Time::now();
-  ros::Duration time_to_publish(10.0);
-  while ( (ros::Time::now() - start) < time_to_publish )
+  ros::Time time_to_publish;
+  try {
+    time_to_publish = start + delay_to_publish;
+  } catch (std::runtime_error & e) {
+    ROS_FATAL_STREAM("It was in the addition: " << e.what() << start.toNSec() << " " << delay_to_publish.toSec()); throw;
+  }
+
+  ROS_FATAL("Starting the loop");
+  for (unsigned i = 0; i < 1000; ++i) //while ( ros::Time::now() < time_to_publish )
   {
     test_roscpp::TestWithHeader msg;
     msg.header.frame_id = "foo";
-    // Don't fill in timestamp so that it defaults to 0.0
 
-    pub.publish(msg);
-    ros::spinOnce();
-    ros::WallDuration(0.01).sleep();
+    try {
+      pub.publish(msg);
+    } catch (std::runtime_error & e) {
+      ROS_FATAL_STREAM("It was in the publish: " << e.what()); throw;
+    }
+    try {
+      ros::spinOnce();
+    } catch (std::runtime_error & e) {
+      ROS_FATAL_STREAM("It was in the spin: " << e.what()); throw;
+    } 
+    try {
+      ros::WallDuration(0.01).sleep();
+    } catch (std::runtime_error & e) {
+      ROS_FATAL_STREAM("It was in the sleep: " << e.what()); throw;
+    } 
   }
+  ROS_FATAL_STREAM("Done testing the message");
 
   SUCCEED();
 }
@@ -68,4 +94,4 @@ int main(int argc, char **argv)
   ros::init(argc, argv, "stamped_topic_statistics_empty_timestamp");
 
   return RUN_ALL_TESTS();
-}
\ No newline at end of file
+}