Add support for simulation reset via a publicly callable API

This allows us to reset simulations without having to call into
gz-transport making the code more readable from an external API.
Depends on #2647

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

Author: arjo129

include/gz/sim/Server.hh

@@ -338,6 +338,9 @@ namespace gz
       /// \brief Stop the server. This will stop all running simulations.
       public: void Stop();
 
+      /// \brief Reset All runners in this simulation
+      public: void ResetAll();
+
       /// \brief Private data
       private: std::unique_ptr<ServerPrivate> dataPtr;
     };

python/src/gz/sim/Server.cc

@@ -46,7 +46,9 @@ void defineSimServer(pybind11::object module)
   .def(
     "is_running",
     pybind11::overload_cast<>(&gz::sim::Server::Running, pybind11::const_),
-    "Get whether the server is running.");
+    "Get whether the server is running.")
+  .def("reset_all", &gz::sim::Server::ResetAll,
+    "Resets all simulation runners under this server.");
 }
 }  // namespace python
 }  // namespace sim

src/Server.cc

@@ -485,6 +485,17 @@ bool Server::RequestRemoveEntity(const Entity _entity,
   return false;
 }
 
+//////////////////////////////////////////////////
+void Server::ResetAll()
+{
+  for (auto worldId = 0u;
+    worldId < this->dataPtr->simRunners.size();
+    worldId++)
+  {
+    this->dataPtr->simRunners[worldId]->Reset(true, false, false);
+  }
+}
+
 //////////////////////////////////////////////////
 void Server::Stop()
 {

src/SimulationRunner.cc

@@ -95,7 +95,6 @@ struct MaybeGilScopedRelease
 #endif
 }
 
-
 //////////////////////////////////////////////////
 SimulationRunner::SimulationRunner(const sdf::World &_world,
                                    const SystemLoaderPtr &_systemLoader,
@@ -1661,3 +1660,17 @@ void SimulationRunner::CreateEntities(const sdf::World &_world)
   // Store the initial state of the ECM;
   this->initialEntityCompMgr.CopyFrom(this->entityCompMgr);
 }
+
+/////////////////////////////////////////////////
+void SimulationRunner::Reset(const bool all,
+  const bool time, const bool model)
+{
+  WorldControl control;
+  std::lock_guard<std::mutex> lock(this->msgBufferMutex);
+  control.rewind = all || time;
+  if (model)
+  {
+    gzwarn << "Model reset not supported" <<std::endl;
+  }
+  this->worldControls.push_back(control);
+}

src/SimulationRunner.hh

@@ -369,6 +369,9 @@ namespace gz
       /// \brief Set the next step to be blocking and paused.
       public: void SetNextStepAsBlockingPaused(const bool value);
 
+      /// \brief Reset the current simulation runner
+      public: void Reset(const bool all, const bool time, const bool model);
+
       /// \brief Updates the physics parameters of the simulation based on the
       /// Physics component of the world, if any.
       public: void UpdatePhysicsParams();

src/TestFixture_TEST.cc

@@ -72,6 +72,8 @@ TEST_F(TestFixtureTest, Callbacks)
   unsigned int preUpdates{0u};
   unsigned int updates{0u};
   unsigned int postUpdates{0u};
+  unsigned int resets{0u};
+
   testFixture.
     OnConfigure([&](const Entity &_entity,
         const std::shared_ptr<const sdf::Element> &_sdf,
@@ -85,20 +87,33 @@ TEST_F(TestFixtureTest, Callbacks)
     {
       this->Updates(_info, _ecm);
       preUpdates++;
-      EXPECT_EQ(preUpdates, _info.iterations);
+      if (resets == 0)
+      {
+        EXPECT_EQ(preUpdates, _info.iterations);
+      }
     }).
     OnUpdate([&](const UpdateInfo &_info, EntityComponentManager &_ecm)
     {
       this->Updates(_info, _ecm);
       updates++;
-      EXPECT_EQ(updates, _info.iterations);
+      if (resets == 0)
+      {
+        EXPECT_EQ(updates, _info.iterations);
+      }
     }).
     OnPostUpdate([&](const UpdateInfo &_info,
         const EntityComponentManager &_ecm)
     {
       this->Updates(_info, _ecm);
       postUpdates++;
-      EXPECT_EQ(postUpdates, _info.iterations);
+      if (resets == 0)
+      {
+        EXPECT_EQ(postUpdates, _info.iterations);
+      }
+    }).
+    OnReset([&](const UpdateInfo &, EntityComponentManager &)
+    {
+      resets++;
     }).
     Finalize();
 
@@ -109,8 +124,19 @@ TEST_F(TestFixtureTest, Callbacks)
   EXPECT_EQ(expectedIterations, preUpdates);
   EXPECT_EQ(expectedIterations, updates);
   EXPECT_EQ(expectedIterations, postUpdates);
+  EXPECT_EQ(0u, resets);
+
+  testFixture.Server()->ResetAll();
+
+  testFixture.Server()->Run(true, expectedIterations, false);
+  EXPECT_EQ(1u, configures);
+  EXPECT_EQ(expectedIterations * 2 - 1, preUpdates);
+  EXPECT_EQ(expectedIterations * 2 - 1, updates);
+  EXPECT_EQ(expectedIterations * 2 - 1, postUpdates);
+  EXPECT_EQ(1u, resets);
 }
 
+
 /////////////////////////////////////////////////
 TEST_F(TestFixtureTest, LoadConfig)
 {