Allow specifying initial simulation time with a CLI argument

include/gz/sim/ServerConfig.hh

@@ -246,6 +246,15 @@ namespace gz
       /// an UpdateRate has not been set.
       public: std::optional<double> UpdateRate() const;
 
+      /// \brief Set the initial simulation time in seconds.
+      /// \param[in] _initialSimTime The desired initial simulation time in
+      /// seconds.
+      public: void SetInitialSimTime(const double &_initialSimTime) const;
+
+      /// \brief Get the initial simulation time in seconds.
+      /// \return The initial simulation time in seconds.
+      public: double InitialSimTime() const;
+
       /// \brief Get whether the server is using the level system
       /// \return True if the server is set to use the level system
       public: bool UseLevels() const;

src/ServerConfig.cc

@@ -248,6 +248,7 @@ class gz::sim::ServerConfigPrivate
           : sdfFile(_cfg->sdfFile),
             sdfString(_cfg->sdfString),
             updateRate(_cfg->updateRate),
+            initialSimTime(_cfg->initialSimTime),
             useLevels(_cfg->useLevels),
             useLogRecord(_cfg->useLogRecord),
             logRecordPath(_cfg->logRecordPath),
@@ -275,6 +276,9 @@ class gz::sim::ServerConfigPrivate
   /// \brief An optional update rate.
   public: std::optional<double> updateRate;
 
+  /// \brief The initial simulation time in seconds.
+  public: double initialSimTime = 0;
+
   /// \brief Use the level system
   public: bool useLevels{false};
 
@@ -389,13 +393,25 @@ std::string ServerConfig::SdfString() const
   return this->dataPtr->sdfString;
 }
 
+//////////////////////////////////////////////////
+void ServerConfig::SetInitialSimTime(const double &_initialSimTime) const
+{
+  this->dataPtr->initialSimTime = _initialSimTime;
+}
+
 //////////////////////////////////////////////////
 void ServerConfig::SetUpdateRate(const double &_hz)
 {
   if (_hz > 0)
     this->dataPtr->updateRate = _hz;
 }
 
+/////////////////////////////////////////////////
+double ServerConfig::InitialSimTime() const
+{
+  return this->dataPtr->initialSimTime;
+}
+
 /////////////////////////////////////////////////
 std::optional<double> ServerConfig::UpdateRate() const
 {

src/SimulationRunner.cc

@@ -122,6 +122,12 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
         static_cast<int>(this->stepSize.count() / this->desiredRtf));
   }
 
+  // Epoch
+  // TODO: Decide on chrono precision.
+  this->simTimeEpoch = std::chrono::round<std::chrono::milliseconds>(
+    std::chrono::duration<double>{_config.InitialSimTime()}
+  );
+
   // World control
   transport::NodeOptions opts;
   std::string ns{"/world/" + this->worldName};
@@ -266,13 +272,13 @@ void SimulationRunner::UpdateCurrentInfo()
   // Rewind
   if (this->requestedRewind)
   {
-    gzdbg << "Rewinding simulation back to time zero." << std::endl;
+    gzdbg << "Rewinding simulation back to initial time." << std::endl;
     this->realTimes.clear();
     this->simTimes.clear();
     this->realTimeFactor = 0;
 
-    this->currentInfo.dt = -this->currentInfo.simTime;
-    this->currentInfo.simTime = std::chrono::steady_clock::duration::zero();
+    this->currentInfo.dt = this->simTimeEpoch - this->currentInfo.simTime;
+    this->currentInfo.simTime = this->simTimeEpoch;
     this->currentInfo.realTime = std::chrono::steady_clock::duration::zero();
     this->currentInfo.iterations = 0;
     this->realTimeWatch.Reset();
@@ -845,8 +851,7 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   this->UpdateSystems();
 
   if (!this->Paused() &&
-       this->requestedRunToSimTime >
-       std::chrono::steady_clock::duration::zero() &&
+       this->requestedRunToSimTime > this->simTimeEpoch &&
        this->currentInfo.simTime >= this->requestedRunToSimTime)
   {
     this->SetPaused(true);
@@ -1104,8 +1109,7 @@ bool SimulationRunner::Stepping() const
 void SimulationRunner::SetRunToSimTime(
     const std::chrono::steady_clock::duration &_time)
 {
-  if (_time >= std::chrono::steady_clock::duration::zero() &&
-      _time > this->currentInfo.simTime)
+  if (_time >= this->simTimeEpoch && _time > this->currentInfo.simTime)
   {
     this->requestedRunToSimTime = _time;
   }
@@ -1355,6 +1359,13 @@ const UpdateInfo &SimulationRunner::CurrentInfo() const
   return this->currentInfo;
 }
 
+/////////////////////////////////////////////////
+const std::chrono::steady_clock::duration &
+  SimulationRunner::SimTimeEpoch() const
+{
+  return this->simTimeEpoch;
+}
+
 /////////////////////////////////////////////////
 const std::chrono::steady_clock::duration &
 SimulationRunner::UpdatePeriod() const

src/SimulationRunner.hh

@@ -192,6 +192,10 @@ namespace gz
       public: void SetUpdatePeriod(
                   const std::chrono::steady_clock::duration &_updatePeriod);
 
+      /// \brief Get the simulation epoch.
+      /// \return The simulation epoch.
+      public: const std::chrono::steady_clock::duration &SimTimeEpoch() const;
+
       /// \brief Get the update period.
       /// \return The update period.
       public: const std::chrono::steady_clock::duration &UpdatePeriod() const;
@@ -427,6 +431,10 @@ namespace gz
       /// The default update rate is 500hz, which is a period of 2ms.
       private: std::chrono::steady_clock::duration updatePeriod{2ms};
 
+      /// \brief The simulation epoch.
+      /// All simulation times will be larger than the epoch. It defaults to 0.
+      private: std::chrono::steady_clock::duration simTimeEpoch{0};
+
       /// \brief List of simulation times used to compute averages.
       private: std::list<std::chrono::steady_clock::duration> simTimes;
 

src/cmd/cmdsim.rb.in

@@ -49,6 +49,8 @@ COMMANDS = { 'sim' =>
   "Available Options:                                                              \n"\
   "  -g                           Run only the GUI.                                \n"\
   "\n"\
+  "  --initial-sim-time [arg]     Initial simulation time, in seconds.             \n"\
+  "\n"\
   "  --iterations [arg]           Number of iterations to execute.                 \n"\
   "\n"\
   "  --levels                     Use the level system. The default is false,      \n"\
@@ -209,6 +211,7 @@ class Cmd
       'file' => '',
       'gui' => 0,
       'hz' => -1,
+      'initial_sim_time' => 0,
       'iterations' => 0,
       'levels' => 0,
       'network_role' => '',
@@ -260,6 +263,10 @@ class Cmd
       opts.on('-z [arg]', Float, 'Update rate in Hertz') do |h|
         options['hz'] = h
       end
+      opts.on('--initial-sim-time [arg]', Float,
+              'Initial simulation time, in seconds.') do |t|
+        options['initial_sim_time'] = t
+      end
       opts.on('-r') do
         options['run'] = 1
       end
@@ -497,15 +504,16 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info."
           ENV['RMT_PORT'] = '1500'
           Process.setpgid(0, 0)
           Process.setproctitle('gz sim server')
-          Importer.runServer(parsed, options['iterations'], options['run'],
-            options['hz'], options['levels'], options['network_role'],
-            options['network_secondaries'], options['record'],
-            options['record-path'], options['record-resources'],
-            options['log-overwrite'], options['log-compress'],
-            options['playback'], options['physics_engine'],
-            options['render_engine_server'], options['render_engine_gui'],
-            options['file'], options['record-topics'].join(':'),
-            options['wait_gui'],
+          Importer.runServer(parsed,
+            options['iterations'], options['run'], options['hz'],
+            options['initial_sim_time'], options['levels'],
+            options['network_role'], options['network_secondaries'],
+            options['record'], options['record-path'],
+            options['record-resources'], options['log-overwrite'],
+            options['log-compress'], options['playback'],
+            options['physics_engine'], options['render_engine_server'],
+            options['render_engine_gui'], options['file'],
+            options['record-topics'].join(':'), options['wait_gui'],
             options['headless-rendering'], options['record-period'])
         end
 
@@ -536,14 +544,15 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info."
       elsif options['server'] == 1
         ENV['RMT_PORT'] = '1500'
         Importer.runServer(parsed, options['iterations'], options['run'],
-            options['hz'], options['levels'], options['network_role'],
-            options['network_secondaries'], options['record'],
-            options['record-path'], options['record-resources'],
-            options['log-overwrite'], options['log-compress'],
-            options['playback'], options['physics_engine'],
-            options['render_engine_server'], options['render_engine_gui'],
-            options['file'], options['record-topics'].join(':'),
-            options['wait_gui'], options['headless-rendering'], options['record-period'])
+            options['hz'], optionas['initial_sim_time'], options['levels'],
+            options['network_role'], options['network_secondaries'],
+            options['record'], options['record-path'],
+            options['record-resources'], options['log-overwrite'],
+            options['log-compress'], options['playback'],
+            options['physics_engine'], options['render_engine_server'],
+            options['render_engine_gui'], options['file'],
+            options['record-topics'].join(':'), options['wait_gui'],
+            options['headless-rendering'], options['record-period'])
             # Otherwise run the gui
       else options['gui']
         if plugin.end_with? ".dylib"

src/gz.cc

@@ -130,7 +130,8 @@ extern "C" const char *findFuelResource(
 
 //////////////////////////////////////////////////
 extern "C" int runServer(const char *_sdfString,
-    int _iterations, int _run, float _hz, int _levels, const char *_networkRole,
+    int _iterations, int _run, float _hz, double _initialSimTime,
+    int _levels, const char *_networkRole,
     int _networkSecondaries, int _record, const char *_recordPath,
     int _recordResources, int _logOverwrite, int _logCompress,
     const char *_playback, const char *_physicsEngine,
@@ -350,6 +351,9 @@ extern "C" int runServer(const char *_sdfString,
   else
     serverConfig.SetSdfFile(_file);
 
+  // Initial simulation time.
+  serverConfig.SetInitialSimTime(_initialSimTime);
+
   // Set the update rate.
   if (_hz > 0.0)
     serverConfig.SetUpdateRate(_hz);

src/gz.hh

@@ -60,7 +60,7 @@ extern "C" GZ_SIM_VISIBLE const char *worldInstallDir();
 /// \param[in] _recordPeriod --record-period option
 /// \return 0 if successful, 1 if not.
 extern "C" GZ_SIM_VISIBLE int runServer(const char *_sdfString,
-    int _iterations, int _run, float _hz, int _levels,
+    int _iterations, int _run, float _hz, double _initialSimTime, int _levels,
     const char *_networkRole, int _networkSecondaries, int _record,
     const char *_recordPath, int _recordResources, int _logOverwrite,
     int _logCompress, const char *_playback,