Apply initial sim time also after a reset.

Signed-off-by: Martin Pecka <[email protected]>
gazebosim · Aug 22, 2023 · 97df15c · 97df15c
1 parent af7d327
commit 97df15c
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 3 deletions.
diff --git a/gazebo/Server.cc b/gazebo/Server.cc
@@ -412,8 +412,10 @@ bool Server::ParseArgs(int _argc, char **_argv)
   {
     try
     {
-      physics::get_world()->SetSimTime(
-          common::Time(this->dataPtr->vm["initial_sim_time"].as<double>()));
+      common::Time initialSimTime {
+        this->dataPtr->vm["initial_sim_time"].as<double>()};
+      physics::get_world()->SetSimTime(initialSimTime);
+      physics::get_world()->SetInitialSimTime(initialSimTime);
       gzmsg << "Setting initial sim time to [" <<
         physics::get_world()->SimTime() << "]\n" << std::endl;
     }

diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc
@@ -1455,7 +1455,7 @@ Light_V World::Lights() const
 //////////////////////////////////////////////////
 void World::ResetTime()
 {
-  this->dataPtr->simTime = common::Time(0);
+  this->dataPtr->simTime = common::Time(this->dataPtr->initialSimTime);
   this->dataPtr->pauseTime = common::Time(0);
   this->dataPtr->startTime = common::Time::GetWallTime();
   this->dataPtr->realTimeOffset = common::Time(0);
@@ -1528,6 +1528,12 @@ void World::SetSimTime(const common::Time &_t)
   this->dataPtr->simTime = _t;
 }
 
+//////////////////////////////////////////////////
+void World::SetInitialSimTime(const common::Time &_t)
+{
+  this->dataPtr->initialSimTime = _t;
+}
+
 //////////////////////////////////////////////////
 gazebo::common::Time World::PauseTime() const
 {

diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh
@@ -240,6 +240,10 @@ namespace gazebo
       /// \return The real time.
       public: common::Time RealTime() const;
 
+      /// \brief Set the initial sim time.
+      /// \param[in] _t The new simulation time
+      public: void SetInitialSimTime(const common::Time &_t);
+
       /// \brief Returns the state of the simulation true if paused.
       /// \return True if paused.
       public: bool IsPaused() const;

diff --git a/gazebo/physics/WorldPrivate.hh b/gazebo/physics/WorldPrivate.hh
@@ -86,6 +86,9 @@ namespace gazebo
       /// \brief Clock time when simulation was started.
       public: common::Time startTime;
 
+      /// \brief Initial simulation time.
+      public: common::Time initialSimTime;
+
       /// \brief True if simulation is paused.
       public: bool pause;
 

diff --git a/test/integration/world_with_initial_sim_time_from_cli.cc b/test/integration/world_with_initial_sim_time_from_cli.cc
@@ -46,8 +46,18 @@ TEST_F(WorldWithInitialSimTimeFromCliTest, CheckInitialSimTime)
 
   // check that the simulation time is the same as the initial sim time
   EXPECT_DOUBLE_EQ(this->world->SimTime().Double(), initialSimTime);
+
+  // check that after a step, the simulation time advances
+  this->world->Step(2);
+  EXPECT_GT(this->world->SimTime().Double() - initialSimTime, 1e-4);
+
+  // check that the simulation time is again the same as the initial sim time
+  // after a reset
+  this->world->Reset();
+  EXPECT_DOUBLE_EQ(this->world->SimTime().Double(), initialSimTime);
 }
 
+
 /////////////////////////////////////////////////
 int main(int argc, char **argv)
 {