From 0b86dda116cf3b90708e08a6b9a52a785dacd546 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20=C5=A0koudlil?= <skoudmar@fel.cvut.cz>
Date: Mon, 8 Jan 2024 14:27:34 +0100
Subject: [PATCH] Enable simulated time when the node is started with
 use_sim_time parameter

add `--ros-args -p use_sim_time:=true` to starting command line
---
 r2r/src/nodes.rs | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs
index 1866185..c0a385f 100644
--- a/r2r/src/nodes.rs
+++ b/r2r/src/nodes.rs
@@ -428,6 +428,29 @@ impl Node {
 
         handlers.push(Box::pin(get_param_types_future));
 
+        #[cfg(feature = "sim-time")]
+        {
+            // create TimeSource based on value of use_sim_time parameter
+            let use_sim_time = {
+                let params = self.params.lock().unwrap();
+                params
+                    .get("use_sim_time")
+                    .and_then(|param| {
+                        if let ParameterValue::Bool(val) = param.value {
+                            Some(val)
+                        } else {
+                            log::error!("Parameter use_sim_time is not bool. Assuming false");
+                            None
+                        }
+                    })
+                    .unwrap_or(false)
+            };
+            if use_sim_time {
+                let ts = self.time_source.clone();
+                ts.enable_sim_time(self)?;
+            }
+        }
+
         // we don't care about the result, the futures will not complete anyway.
         Ok((join_all(handlers).map(|_| ()), event_rx))
     }