diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index 2eaae435eb..f6dc3df92b 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -246,6 +246,15 @@ namespace gz /// an UpdateRate has not been set. public: std::optional 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; diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 5461e18707..b7e79b0cde 100644 --- a/src/ServerConfig.cc +++ b/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 updateRate; + /// \brief The initial simulation time in seconds. + public: double initialSimTime = 0; + /// \brief Use the level system public: bool useLevels{false}; @@ -389,6 +393,12 @@ 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) { @@ -396,6 +406,12 @@ void ServerConfig::SetUpdateRate(const double &_hz) this->dataPtr->updateRate = _hz; } +///////////////////////////////////////////////// +double ServerConfig::InitialSimTime() const +{ + return this->dataPtr->initialSimTime; +} + ///////////////////////////////////////////////// std::optional ServerConfig::UpdateRate() const { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index cc7ec019d5..4504bb95b4 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -126,6 +126,12 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, static_cast(this->stepSize.count() / this->desiredRtf)); } + // Epoch + this->simTimeEpoch = std::chrono::round( + std::chrono::duration{_config.InitialSimTime()} + ); + this->currentInfo.simTime = this->simTimeEpoch; + // World control transport::NodeOptions opts; std::string ns{"/world/" + this->worldName}; @@ -271,13 +277,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(); @@ -290,22 +296,23 @@ void SimulationRunner::UpdateCurrentInfo() } // Seek - if (this->requestedSeek >= std::chrono::steady_clock::duration::zero()) + if (this->requestedSeek && this->requestedSeek.value() >= this->simTimeEpoch) { gzdbg << "Seeking to " << std::chrono::duration_cast( - this->requestedSeek).count() << "s." << std::endl; + this->requestedSeek.value()).count() << "s." << std::endl; this->realTimes.clear(); this->simTimes.clear(); this->realTimeFactor = 0; - this->currentInfo.dt = this->requestedSeek - this->currentInfo.simTime; - this->currentInfo.simTime = this->requestedSeek; + this->currentInfo.dt = this->requestedSeek.value() - + this->currentInfo.simTime; + this->currentInfo.simTime = this->requestedSeek.value(); this->currentInfo.iterations = 0; this->currentInfo.realTime = this->realTimeWatch.ElapsedRunTime(); - this->requestedSeek = std::chrono::steady_clock::duration{-1}; + this->requestedSeek = {}; return; } @@ -849,13 +856,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Update all the systems. this->UpdateSystems(); - if (!this->Paused() && - this->requestedRunToSimTime > - std::chrono::steady_clock::duration::zero() && - this->currentInfo.simTime >= this->requestedRunToSimTime) + if (!this->Paused() && this->requestedRunToSimTime && + this->requestedRunToSimTime.value() > this->simTimeEpoch && + this->currentInfo.simTime >= this->requestedRunToSimTime.value()) { this->SetPaused(true); - this->requestedRunToSimTime = std::chrono::steady_clock::duration{-1}; + this->requestedRunToSimTime = {}; } if (!this->Paused() && this->pendingSimIterations > 0) @@ -1109,14 +1115,13 @@ 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; } else { - this->requestedRunToSimTime = std::chrono::seconds(-1); + this->requestedRunToSimTime = {}; } } @@ -1258,7 +1263,7 @@ void SimulationRunner::ProcessWorldControl() this->requestedRewind = control.rewind; // Seek - if (control.seek >= std::chrono::steady_clock::duration::zero()) + if (control.seek >= this->simTimeEpoch) { this->requestedSeek = control.seek; } @@ -1360,6 +1365,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 diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 81ab164403..aac9c0d6a7 100644 --- a/src/SimulationRunner.hh +++ b/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; @@ -217,8 +221,8 @@ namespace gz /// \brief Set the run to simulation time. /// \param[in] _time A simulation time in the future to run to and then - /// pause. A negative number or a time less than the current simulation - /// time disables the run-to feature. + /// pause. A time prior than the current simulation time disables the + /// run-to feature. public: void SetRunToSimTime( const std::chrono::steady_clock::duration &_time); @@ -432,6 +436,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 simTimes; @@ -489,12 +497,12 @@ namespace gz private: bool requestedRewind{false}; /// \brief If user asks to seek to a specific sim time, this holds the - /// time.s A negative value means there's no request from the user. - private: std::chrono::steady_clock::duration requestedSeek{-1}; + /// time. + private: std::optional requestedSeek; - /// \brief A simulation time in the future to run to and then pause. - /// A negative number indicates that this variable it not being used. - private: std::chrono::steady_clock::duration requestedRunToSimTime{-1}; + /// \brief A simulation time past the epoch to run to and then pause. + private: std::optional + requestedRunToSimTime; /// \brief Keeps the latest simulation info. private: UpdateInfo currentInfo; diff --git a/src/cmd/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index 2f7e6e3ba3..9e7cdd52ee 100755 --- a/src/cmd/cmdsim.rb.in +++ b/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 @@ -460,7 +467,7 @@ Please use [GZ_SIM_RESOURCE_PATH] instead." end # Import the runServer function - Importer.extern 'int runServer(const char *, int, int, float, int, + Importer.extern 'int runServer(const char *, int, int, float, double, int, const char *, int, int, const char *, int, int, int, const char *, const char *, const char *, const char *, const char *, @@ -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'], 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']) # Otherwise run the gui else options['gui'] if plugin.end_with? ".dll" diff --git a/src/gz.cc b/src/gz.cc index d1e0e21a51..aa0f908518 100644 --- a/src/gz.cc +++ b/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); diff --git a/src/gz.hh b/src/gz.hh index e2d7da672d..7fefb37254 100644 --- a/src/gz.hh +++ b/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, diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 73dbd7de1d..946dc6a079 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include #include "test_config.hh" // NOLINT(build/include) @@ -129,6 +131,31 @@ TEST(CmdLine, GazeboServer) } } +///////////////////////////////////////////////// +TEST(CmdLine, SimtimeArgument) +{ + std::string cmd = + kGzCommand + " -r -v 4 --iterations 100 --initial-sim-time 1000.5 " + + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; + + std::cout << "Running command [" << cmd << "]" << std::endl; + int msgCount = 0; + + gz::transport::Node node; + auto cb = [&](const gz::msgs::Clock &_msg) -> void + { + EXPECT_GE(_msg.sim().sec() + _msg.sim().nsec()/1000000000.0, + 1000.5); + msgCount++; + }; + + auto cbFcn = std::function(cb); + EXPECT_TRUE(node.Subscribe(std::string("/clock"), cbFcn)); + + std::string output = customExecStr(cmd); + EXPECT_GT(msgCount, 0); +} + ///////////////////////////////////////////////// TEST(CmdLine, Gazebo) {