Fixing assorted problems with the accidental inclusion of messages with invalid times

tools/rosbag/src/bag.cpp

@@ -449,7 +449,7 @@ void Bag::readChunkHeader(ChunkHeader& chunk_header) const {
         throw BagFormatException("Error reading CHUNK record");
         
     M_string& fields = *header.getValues();
-    
+
     if (!isOp(fields, OP_CHUNK))
         throw BagFormatException("Expected CHUNK op not found");
 
@@ -542,7 +542,13 @@ void Bag::readTopicIndexRecord102() {
 
         ROS_DEBUG("  - %d.%d: %llu", sec, nsec, (unsigned long long) index_entry.chunk_pos);
 
-        connection_index.insert(connection_index.end(), index_entry);
+        if (index_entry.time < ros::TIME_MIN || index_entry.time > ros::TIME_MAX)
+        {
+          ROS_ERROR("Index entry for topic %s contains invalid time.", topic.c_str());
+        } else
+        {
+          connection_index.insert(connection_index.end(), index_entry);
+        }
     }
 }
 
@@ -584,7 +590,13 @@ void Bag::readConnectionIndexRecord200() {
 
         ROS_DEBUG("  - %d.%d: %llu+%d", sec, nsec, (unsigned long long) index_entry.chunk_pos, index_entry.offset);
 
-        connection_index.insert(connection_index.end(), index_entry);
+        if (index_entry.time < ros::TIME_MIN || index_entry.time > ros::TIME_MAX)
+        {
+          ROS_ERROR("Index entry for topic %s contains invalid time.  This message will not be loaded.", connections_[connection_id]->topic.c_str());
+        } else
+        {
+          connection_index.insert(connection_index.end(), index_entry);
+        }
     }
 }
 

tools/rosbag/src/player.cpp

@@ -121,9 +121,19 @@ void Player::publish() {
       puts("");
     
     // Publish all messages in the bags
+    View full_view;
+    foreach(shared_ptr<Bag> bag, bags_)
+        full_view.addQuery(*bag);
+
+    ros::Time initial_time = full_view.getBeginTime();
+
+    initial_time += ros::Duration(options_.time);
+
     View view;
     foreach(shared_ptr<Bag> bag, bags_)
-        view.addQuery(*bag);
+      view.addQuery(*bag, initial_time, ros::TIME_MAX);
+
+
 
     // Advertise all of our messages
     foreach(const ConnectionInfo* c, view.getConnections())

tools/rosbag/src/recorder.cpp

@@ -146,6 +146,8 @@ int Recorder::run() {
     else
         record_thread = boost::thread(boost::bind(&Recorder::doRecord, this));
 
+    ros::Time::waitForValid();
+
     // Subscribe to the snapshot trigger
     ros::Subscriber trigger_sub = nh.subscribe<std_msgs::Empty>("snapshot_trigger", 100, boost::bind(&Recorder::snapshotTrigger, this, _1));
 

tools/rosbag/src/view.cpp

@@ -266,21 +266,24 @@ void View::updateQueries(BagQuery* q) {
           }
         }
 
-        // todo: make faster with a map of maps
-        bool found = false;
-        for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
-            MessageRange* r = *k;
-
-            // If the topic and query are already in our ranges, we update
-            if (r->bag_query == q && r->connection_info->id == connection->id) {
-                r->begin = begin;
-                r->end   = end;
-                found    = true;
-                break;
+        if (begin != end)
+        {
+            // todo: make faster with a map of maps
+            bool found = false;
+            for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
+                MessageRange* r = *k;
+                
+                // If the topic and query are already in our ranges, we update
+                if (r->bag_query == q && r->connection_info->id == connection->id) {
+                    r->begin = begin;
+                    r->end   = end;
+                    found    = true;
+                    break;
+                }
             }
+            if (!found)
+                ranges_.push_back(new MessageRange(begin, end, connection, q));
         }
-        if (!found)
-        	ranges_.push_back(new MessageRange(begin, end, connection, q));
     }
 
     view_revision_++;

tools/rosbag/test/test_bag.cpp

@@ -178,17 +178,26 @@ TEST(rosbag, simple_write_and_read_works) {
     topics.push_back(std::string("chatter"));
     topics.push_back(std::string("numbers"));
 
+    int count = 0;
     rosbag::View view(b2, rosbag::TopicQuery(topics));
     foreach(rosbag::MessageInstance const m, view) {
         std_msgs::String::ConstPtr s = m.instantiate<std_msgs::String>();
         if (s != NULL)
+        {
+            count++;
             ASSERT_EQ(s->data, std::string("foo"));
+        }
 
         std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
         if (i != NULL)
+        {
+            count++;
             ASSERT_EQ(i->data, 42);
+        }
     }
 
+    ASSERT_EQ(count,2);
+
     b2.close();
 }