From fa0d8b6c17d89022eb7db5ffc3954c82e0ed8605 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Sat, 20 Apr 2024 10:43:23 +0200
Subject: [PATCH] simtime is now initialized form wall time

---
 config/multirotor_simulator.yaml |  3 +++
 src/multirotor_simulator.cpp     | 15 ++++++++++++---
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/config/multirotor_simulator.yaml b/config/multirotor_simulator.yaml
index 9c69c9b..6e7bac6 100644
--- a/config/multirotor_simulator.yaml
+++ b/config/multirotor_simulator.yaml
@@ -2,6 +2,9 @@ simulation_rate: 100.0 # Hz
 clock_rate: 100.0 # Hz
 realtime_factor: 1.0 # [-]
 
+# when true, the simulation time will get initialized from wall time
+sim_time_from_wall_time: true
+
 g: 9.81 # [ms^-2]
 
 iterate_without_input: true
diff --git a/src/multirotor_simulator.cpp b/src/multirotor_simulator.cpp
index 95909ba..412c4cb 100644
--- a/src/multirotor_simulator.cpp
+++ b/src/multirotor_simulator.cpp
@@ -106,9 +106,6 @@ void MultirotorSimulator::onInit() {
 
   srand(time(NULL));
 
-  sim_time_            = ros::Time(0);
-  last_published_time_ = ros::Time(0);
-
   mrs_lib::ParamLoader param_loader(nh_, "MultirotorSimulator");
 
   std::string custom_config_path;
@@ -132,6 +129,18 @@ void MultirotorSimulator::onInit() {
   double clock_rate;
   param_loader.loadParam("clock_rate", clock_rate);
 
+  bool sim_time_from_wall_time;
+  param_loader.loadParam("sim_time_from_wall_time", sim_time_from_wall_time);
+
+  if (sim_time_from_wall_time) {
+    sim_time_ = ros::Time(ros::WallTime::now().toSec());
+  } else {
+    sim_time_ = ros::Time(0);
+  }
+
+  last_published_time_  = sim_time_;
+  last_sim_time_status_ = sim_time_;
+
   drs_params_.paused = false;
 
   std::vector<std::string> uav_names;