Restore log playback

This is related to: gazebosim/gazebo_test_cases#1640

During the refactor of the SimulationRunner we seem to have broken log
playback. In our current set up we need a sdf world to start a
SimulationRunner. This sdf world is used for parameterizing which
services and topics the server should advertise on. The client GUI then
queries the server for unning worlds derived from SDFs.

Unfortunately, when playing back from a log file, no SDF world is
available. All the world information comes directly from the log file.
Unfortunately, this means that when we start a playback session the
server always responds to the client with a default world. The client
then proceeds to listen for the state on `/world/default/state`, however
once the server proceeds to run the log files, it publishes the state on
`/world/log_pendulum/state`.

The current work around this PR proposes is to read the log file during
initialization prior to setting up the transport topics. We then set up
the relevant topics. There are probably better ways of handling such
behaviour, but given the limited time this seemed to be the quickest way
to restore functionality.

Signed-off-by: Arjo Chakravarty <[email protected]>

src/CMakeLists.txt

@@ -222,6 +222,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME}
   gz-rendering${GZ_RENDERING_VER}::core
   gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER}
   gz-transport${GZ_TRANSPORT_VER}::parameters
+  gz-transport${GZ_TRANSPORT_VER}::log
   sdformat${SDF_VER}::sdformat${SDF_VER}
   protobuf::libprotobuf
   PRIVATE

src/Server.cc

@@ -24,6 +24,11 @@
 #include <gz/common/SystemPaths.hh>
 #include <gz/fuel_tools/Interface.hh>
 #include <gz/fuel_tools/ClientConfig.hh>
+#include <gz/transport/log/Log.hh>
+
+#include "gz/sim/components/World.hh"
+#include "gz/sim/components/Name.hh"
+
 #include <sdf/Root.hh>
 #include <sdf/Error.hh>
 #include <sdf/ParserConfig.hh>
@@ -58,6 +63,87 @@ struct DefaultWorld
   }
 };
 
+
+namespace {
+// A slow and ugly hack to restore Log playback
+std::optional<std::string>
+  WorldFromLog(std::string logPath)
+{
+  if (common::isFile(logPath))
+  {
+    // TODO(arjo): Restore Zip-file
+    return std::nullopt;
+  }
+
+  std::string dbPath = common::joinPaths(logPath,
+    "state.tlog");
+  gzmsg << "Loading log file [" + dbPath + "]\n";
+  if (!common::exists(dbPath))
+  {
+    gzerr << "Log path invalid. File [" << dbPath << "] "
+      << "does not exist. Nothing to play.\n";
+    return std::nullopt;
+  }
+
+  auto log = std::make_unique<transport::log::Log>();
+  if (!log->Open(dbPath))
+  {
+    gzerr << "Failed to open log file [" << dbPath << "]" << std::endl;
+    return std::nullopt;
+  }
+
+  auto batch = log->QueryMessages();
+  auto iter = batch.begin();
+
+  if (iter == batch.end())
+  {
+    gzerr << "No messages found in log file [" << dbPath << "]" << std::endl;
+  }
+
+  EntityComponentManager tempEcm;
+  // Look for the first SerializedState message and use it to set the initial
+  // state of the world. Messages received before this are ignored.
+  for (; iter != batch.end(); ++iter)
+  {
+    auto msgType = iter->Type();
+    if (msgType == "gz.msgs.SerializedState")
+    {
+      msgs::SerializedState msg;
+      msg.ParseFromString(iter->Data());
+      tempEcm.SetState(msg);
+      break;
+    }
+    else if (msgType == "gz.msgs.SerializedStateMap")
+    {
+      msgs::SerializedStateMap msg;
+      msg.ParseFromString(iter->Data());
+      tempEcm.SetState(msg);
+      break;
+    }
+  }
+  auto worldEntity = tempEcm.EntityByComponents(components::World());
+  if (kNullEntity == worldEntity)
+  {
+    gzerr << "Missing world entity." << std::endl;
+    return std::nullopt;
+  }
+
+  auto name = tempEcm.ComponentData<components::Name>(worldEntity);
+  if (!name.has_value())
+  {
+    return std::nullopt;
+  }
+
+  std::stringstream worldTemplate;
+  worldTemplate << "<sdf version='1.6'>"
+        << "<world name='"<< name.value() <<"'>"
+        << "</world>"
+        << "</sdf>";
+  return worldTemplate.str();
+}
+
+}
+
 /////////////////////////////////////////////////
 Server::Server(const ServerConfig &_config)
   : dataPtr(new ServerPrivate)
@@ -190,10 +276,20 @@ Server::Server(const ServerConfig &_config)
     case ServerConfig::SourceType::kNone:
     default:
     {
-      gzmsg << "Loading default world.\n";
-      // Load an empty world.
-      /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root.
-      errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World());
+      if (_config.LogPlaybackPath() == "")
+      {
+        // Load an empty world.
+        /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root.
+        errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World());
+      }
+      else
+      {
+        auto world = WorldFromLog(_config.LogPlaybackPath());
+        if (world.has_value())
+          this->dataPtr->sdfRoot.LoadSdfString(world.value());
+        else
+          return;
+      }
       break;
     }
   }