Set initial sim time from the command-line (#3294)

Signed-off-by: Audrow Nash <audrow.j.nash@nasa.gov>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Steve Peters <computersthatmove@gmail.com>

gazebo/Server.cc

@@ -198,6 +198,8 @@ bool Server::ParseArgs(int _argc, char **_argv)
      "Recording filter (supports wildcard and regular expression).")
     ("record_resources", "Recording with model meshes and materials.")
     ("seed",  po::value<double>(), "Start with a given random number seed.")
+    ("initial_sim_time", po::value<double>(),
+     "Initial simulation time (seconds).")
     ("iters",  po::value<unsigned int>(), "Number of iterations to simulate.")
     ("minimal_comms", "Reduce the TCP/IP traffic output by gzserver")
     ("server-plugin,s", po::value<std::vector<std::string> >(),
@@ -406,6 +408,23 @@ bool Server::ParseArgs(int _argc, char **_argv)
     }
   }
 
+  if (this->dataPtr->vm.count("initial_sim_time"))
+  {
+    try
+    {
+      physics::get_world()->SetSimTime(
+          common::Time(this->dataPtr->vm["initial_sim_time"].as<double>()));
+      gzmsg << "Setting initial sim time to [" <<
+        physics::get_world()->SimTime() << "]\n" << std::endl;
+    }
+    catch(...)
+    {
+      gzerr << "Unable to cast initial_sim_time[" <<
+        this->dataPtr->vm["initial_sim_time"].as<double>() << "] "
+        << "to double for setting initial sim time\n" << std::endl;
+    }
+  }
+
   this->ProcessParams();
 
   return true;

test/integration/CMakeLists.txt

@@ -121,6 +121,7 @@ set(tests
   world_entity_below_point.cc
   world_playback.cc
   world_population.cc
+  world_with_initial_sim_time_from_cli.cc
   worlds_installed.cc
   )
 

test/integration/world_with_initial_sim_time_from_cli.cc

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <string>
+#include "gazebo/common/Time.hh"
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+
+const double initialSimTime = 1675117690.123456;
+
+/// \brief Helper class that initializes each test.
+class WorldWithInitialSimTimeFromCliTest : public ServerFixture
+{
+  /// \brief Class constructor.
+  public: WorldWithInitialSimTimeFromCliTest()
+  {
+    // Start the server with an inital sim time and paused.
+    this->LoadArgs("-u --initial_sim_time " + std::to_string(initialSimTime));
+    this->world = physics::get_world("default");
+  }
+
+  /// \brief Pointer to the world.
+  public: physics::WorldPtr world;
+};
+
+/////////////////////////////////////////////////
+/// \brief Check that the initial simulation time is set correctly.
+TEST_F(WorldWithInitialSimTimeFromCliTest, CheckInitialSimTime)
+{
+  ASSERT_TRUE(this->world != NULL);
+  EXPECT_TRUE(this->world->IsPaused());
+
+  // check that the simulation time is the same as the initial sim time
+  EXPECT_DOUBLE_EQ(this->world->SimTime().Double(), initialSimTime);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}