Fixes flaky RecordAndPlayback test in INTEGRATION_log_system (#463)

The flakiness comes from two sources:
1. Poses recorded by the LogRecorder are published by the
   SceneBroadcaster system throttled at 60 Hz. The throttle mechanism
   uses real-time instead of sim-time which causes a variance in the
   number of recorded poses from run to run. However, the expected number of
   recorded poses was calculated with the assumption that the simulation
   would run with a 1.0 RTF. If the CPU load is high, there could be a
   mismatch between the expected and the actual number of recorded
   poses, which causes the test to fail.
   This can be checked by running the test with
`cpulimit -l 20 -f  bin/INTEGRATION_log_system  -- --gtest_filter="*RecordAndPlayback"`

2. An attempt is made to match up played back poses with the closest
   timestamp in the recorded file. These poses are again published by
   the SceneBroadcaster, so they are subject to the same kind of timing
   issues as the recorded poses.

The solution in this patch for the first issue is to determine the
expected number of recorded poses by counting them in a separate system
that mimics the throttling mechanism of ign-transport. For the second
issue, a testing system is added to the playback server bypassing the
SceSceneBroadcaster altogether.

Signed-off-by: Addisu Z. Taddese <[email protected]>

Author: azeey

test/integration/log_system.cc

@@ -707,6 +707,9 @@ TEST_F(LogSystemTest, RecordAndPlayback)
   // Create temp directory to store log
   this->CreateLogsDir();
 
+  // Used to count the expected number of poses recorded. Counting is necessary
+  // as the number varied depending on the CPU load.
+  int expectedPoseCount = 0;
   // Record
   {
     // World with moving entities
@@ -725,6 +728,23 @@ TEST_F(LogSystemTest, RecordAndPlayback)
     recordServerConfig.SetSdfString(recordSdfRoot.Element()->ToString(""));
     Server recordServer(recordServerConfig);
 
+    std::chrono::steady_clock::time_point lastPoseTime;
+    const int poseHz = 60;
+    const std::chrono::duration<double> msgPeriod{1.0 / poseHz};
+    // This system counts the expected number of poses recorded by reproducing
+    // the throttle mechanism used by ign-transport.
+    test::Relay recordedPoseCounter;
+    recordedPoseCounter.OnPostUpdate(
+        [&](const UpdateInfo &, const EntityComponentManager &)
+        {
+          auto tNow = std::chrono::steady_clock::now();
+          if ((tNow - lastPoseTime) > msgPeriod)
+          {
+            lastPoseTime = tNow;
+            ++expectedPoseCount;
+          }
+        });
+    recordServer.AddSystem(recordedPoseCounter.systemPtr);
     // Run for a few seconds to record different poses
     recordServer.Run(true, 1000, false);
   }
@@ -814,98 +834,84 @@ TEST_F(LogSystemTest, RecordAndPlayback)
   // Callback function for entities played back
   // Compare current pose being played back with the pose with the closest
   //   timestamp in the recorded file.
-  std::function<void(const msgs::Pose_V &)> msgCb =
-      [&](const msgs::Pose_V &_playedMsg) -> void
-  {
-    // Playback continues even after the log file is over
-    if (batch.end() == recordedIter)
-      return;
-
-    ASSERT_TRUE(_playedMsg.has_header());
-    ASSERT_TRUE(_playedMsg.header().has_stamp());
-    EXPECT_EQ(0, _playedMsg.header().stamp().sec());
-
-    // Get next recorded message
-    EXPECT_EQ("ignition.msgs.Pose_V", recordedIter->Type());
-    recordedMsg.ParseFromString(recordedIter->Data());
-
-    ASSERT_TRUE(recordedMsg.has_header());
-    ASSERT_TRUE(recordedMsg.header().has_stamp());
-    EXPECT_EQ(0, recordedMsg.header().stamp().sec());
-
-    // Log clock timestamp matches message timestamp
-    EXPECT_EQ(recordedMsg.header().stamp().nsec(),
-        recordedIter->TimeReceived().count());
-
-    // Dynamic poses are throttled according to real time during playback,
-    // so we can't guarantee the exact timestamp as recorded.
-    EXPECT_NEAR(_playedMsg.header().stamp().nsec(),
-        recordedMsg.header().stamp().nsec(), 100000000);
-
-    // Loop through all recorded poses, update map
-    std::map<std::string, msgs::Pose> entityRecordedPose;
-    for (int i = 0; i < recordedMsg.pose_size(); ++i)
-    {
-      entityRecordedPose[recordedMsg.pose(i).name()] = recordedMsg.pose(i);
-    }
+  test::Relay playbackPoseTester;
+  playbackPoseTester.OnPostUpdate(
+      [&](const UpdateInfo &_info, const EntityComponentManager &_ecm)
+      {
+        // Playback continues even after the log file is over
+        if (batch.end() == recordedIter)
+          return;
+
+        // Get next recorded message
+        EXPECT_EQ("ignition.msgs.Pose_V", recordedIter->Type());
+        recordedMsg.ParseFromString(recordedIter->Data());
+
+        ASSERT_TRUE(recordedMsg.has_header());
+        ASSERT_TRUE(recordedMsg.header().has_stamp());
+        EXPECT_EQ(0, recordedMsg.header().stamp().sec());
+
+        // Log clock timestamp matches message timestamp
+        EXPECT_EQ(recordedMsg.header().stamp().nsec(),
+            recordedIter->TimeReceived().count());
+
+        // A recorded messages is matched when its timestamp is within 100us
+        // of the current iteration's sim time.
+        if (std::abs((_info.simTime - recordedIter->TimeReceived()).count()) >
+            100000)
+        {
+          return;
+        }
 
-    // Has 6 dynamic entities: 4 in dbl pendulum and 2 in nested model
-    EXPECT_EQ(6, _playedMsg.pose().size());
-    EXPECT_EQ(6u, entityRecordedPose.size());
+        // Has 6 dynamic entities: 4 in dbl pendulum and 2 in nested model
+        EXPECT_EQ(6, recordedMsg.pose_size());
 
-    // Loop through all entities and compare played poses to recorded ones
-    for (int i = 0; i < _playedMsg.pose_size(); ++i)
-    {
-      auto posePlayed = msgs::Convert(_playedMsg.pose(i));
-      auto poseRecorded = msgs::Convert(
-          entityRecordedPose[_playedMsg.pose(i).name()]);
-
-      EXPECT_NEAR(posePlayed.Pos().X(), poseRecorded.Pos().X(), 0.1)
-          << _playedMsg.pose(i).name();
-      EXPECT_NEAR(posePlayed.Pos().Y(), poseRecorded.Pos().Y(), 0.1)
-          << _playedMsg.pose(i).name();
-      EXPECT_NEAR(posePlayed.Pos().Z(), poseRecorded.Pos().Z(), 0.1)
-          << _playedMsg.pose(i).name();
-
-      EXPECT_NEAR(posePlayed.Rot().Roll(), poseRecorded.Rot().Roll(), 0.1)
-          << _playedMsg.pose(i).name();
-      EXPECT_NEAR(posePlayed.Rot().Pitch(), poseRecorded.Rot().Pitch(), 0.1)
-          << _playedMsg.pose(i).name();
-      EXPECT_NEAR(posePlayed.Rot().Yaw(), poseRecorded.Rot().Yaw(), 0.1)
-          << _playedMsg.pose(i).name();
-    }
+        // Loop through all recorded poses, and check them against the
+        // playedback poses.
+        for (int i = 0; i < recordedMsg.pose_size(); ++i)
+        {
+          const math::Pose3d &poseRecorded = msgs::Convert(recordedMsg.pose(i));
+          const std::string &name = recordedMsg.pose(i).name();
+
+          auto entity = _ecm.EntityByComponents(
+              components::Name(recordedMsg.pose(i).name()));
+          ASSERT_NE(kNullEntity, entity);
+          auto poseComp = _ecm.Component<components::Pose>(entity);
+          ASSERT_NE(nullptr, poseComp);
+          const auto &posePlayed = poseComp->Data();
+
+          EXPECT_NEAR(posePlayed.Pos().X(), poseRecorded.Pos().X(), 0.1)
+            << name;
+          EXPECT_NEAR(posePlayed.Pos().Y(), poseRecorded.Pos().Y(), 0.1)
+            << name;
+          EXPECT_NEAR(posePlayed.Pos().Z(), poseRecorded.Pos().Z(), 0.1)
+            << name;
+
+          EXPECT_NEAR(posePlayed.Rot().Roll(), poseRecorded.Rot().Roll(), 0.1)
+            << name;
+          EXPECT_NEAR(posePlayed.Rot().Pitch(), poseRecorded.Rot().Pitch(), 0.1)
+            << name;
+          EXPECT_NEAR(posePlayed.Rot().Yaw(), poseRecorded.Rot().Yaw(), 0.1)
+            << name;
+        }
 
-    ++recordedIter;
-    nTotal++;
-  };
+        ++recordedIter;
+      });
 
-  // Subscribe to ignition topic and compare to logged poses
-  transport::Node node;
-  // TODO(louise) The world name should match the recorded world
-  node.Subscribe("/world/default/dynamic_pose/info", msgCb);
-
-  int playbackSteps = 500;
-  int poseHz = 60;
-  int expectedPoseCount = playbackSteps * 1e-3 / (1.0/poseHz);
-  // Run for a few seconds to play back different poses
-  playServer.Run(true, playbackSteps, false);
-
-  int sleep = 0;
-  int maxSleep = 30;
-  for (; nTotal < expectedPoseCount && sleep < maxSleep; ++sleep)
-  {
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  }
-  EXPECT_NE(maxSleep, sleep);
+  playServer.AddSystem(playbackPoseTester.systemPtr);
+
+  // Playback a subset of the log file and check for poses. Note: the poses are
+  // checked in the playbackPoseTester
+  playServer.Run(true, 500, false);
+
+  // Count the total number of pose messages in the log file
+  for (auto it = batch.begin(); it != batch.end(); ++it, ++nTotal) { }
 
-  #if !defined (__APPLE__)
-  /// \todo(anyone) there seems to be a race condition that sometimes cause an
-  /// additional messages to be published by the scene broadcaster
-  // 60Hz
-  EXPECT_TRUE(nTotal == expectedPoseCount || nTotal == expectedPoseCount + 1)
+  // The expectedPoseCount might be off by ±2 because the ign transport's
+  // throttle mechanism (which is used by the SceneBroadcaster when publishing
+  // poses) uses real-time
+  EXPECT_LE(std::abs(nTotal - expectedPoseCount), 2)
       << "nTotal [" << nTotal << "] expectedPoseCount [" << expectedPoseCount
       << "]";
-  #endif
 
   this->RemoveLogsDir();
 }