From 97df15cdfdc2f6ce7ca835ec8d206b22e21333d7 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 22 Aug 2023 10:26:36 +0200 Subject: [PATCH] Apply initial sim time also after a reset. Signed-off-by: Martin Pecka --- gazebo/Server.cc | 6 ++++-- gazebo/physics/World.cc | 8 +++++++- gazebo/physics/World.hh | 4 ++++ gazebo/physics/WorldPrivate.hh | 3 +++ .../world_with_initial_sim_time_from_cli.cc | 10 ++++++++++ 5 files changed, 28 insertions(+), 3 deletions(-) diff --git a/gazebo/Server.cc b/gazebo/Server.cc index 043b50586e..f601b1d85f 100644 --- 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())); + common::Time initialSimTime { + this->dataPtr->vm["initial_sim_time"].as()}; + 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 index f64078424e..714be31478 100644 --- 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 index 3070cdb5c5..2284318f01 100644 --- 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 index 694813c61e..1883aa0111 100644 --- 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 index 2a03034fd4..ca82f1c7cd 100644 --- 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) {