From 409241592ac5de7262e94d495d60dc4d5689b321 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 4 Sep 2024 17:10:07 +0800 Subject: [PATCH 1/2] Restore log playback This is related to: https://github.com/gazebosim/gazebo_test_cases/issues/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 --- src/CMakeLists.txt | 1 + src/Server.cc | 104 +++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 101 insertions(+), 4 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index d4a5589b19..c10321c702 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/Server.cc b/src/Server.cc index 712f273467..657361bcf0 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -24,6 +24,11 @@ #include #include #include +#include + +#include "gz/sim/components/World.hh" +#include "gz/sim/components/Name.hh" + #include #include #include @@ -58,6 +63,87 @@ struct DefaultWorld } }; + +namespace { +// A slow and ugly hack to restore Log playback +std::optional + 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(); + 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(worldEntity); + if (!name.has_value()) + { + return std::nullopt; + } + + std::stringstream worldTemplate; + worldTemplate << "" + << "" + << "" + << ""; + 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; } } From e77bf7769e018d6c362c63cb9403fcf6010e9f01 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 5 Sep 2024 14:22:00 +0800 Subject: [PATCH 2/2] Make tests pass (hopefully) Signed-off-by: Arjo Chakravarty --- src/Server.cc | 9 ++++++--- test/integration/log_system.cc | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 657361bcf0..3cf2276e53 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -98,6 +98,7 @@ std::optional if (iter == batch.end()) { gzerr << "No messages found in log file [" << dbPath << "]" << std::endl; + return std::nullopt; } EntityComponentManager tempEcm; @@ -124,14 +125,16 @@ std::optional auto worldEntity = tempEcm.EntityByComponents(components::World()); if (kNullEntity == worldEntity) { - gzerr << "Missing world entity." << std::endl; - return std::nullopt; + // Seems like levels logs can start without a world component + gzwarn << "Log file has no initial world component.\n"; + return DefaultWorld::World(); } auto name = tempEcm.ComponentData(worldEntity); if (!name.has_value()) { - return std::nullopt; + gzwarn << "Log file world has no name component.\n"; + return DefaultWorld::World(); } std::stringstream worldTemplate; diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index f867ca731a..1fa2ea6ab7 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -904,7 +904,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogControl)) msgs::Boolean res; bool result{false}; unsigned int timeout = 1000; - std::string service{"/world/default/playback/control"}; + std::string service{"/world/shapes/playback/control"}; for (auto i : secs) { req.mutable_seek()->set_sec(i);