From 62897e2afe83c6d4c3b7985b1d2d8db1dcf6462b 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:18:17 +0100
Subject: [PATCH] Add examples showing usage of sim time

---
 r2r/Cargo.toml                     |  4 ++
 r2r/examples/sim_time_publisher.rs | 30 +++++++++++
 r2r/examples/timer_sim_time.rs     | 82 ++++++++++++++++++++++++++++++
 r2r/src/time_source.rs             |  2 +
 4 files changed, 118 insertions(+)
 create mode 100644 r2r/examples/sim_time_publisher.rs
 create mode 100644 r2r/examples/timer_sim_time.rs

diff --git a/r2r/Cargo.toml b/r2r/Cargo.toml
index 4674615..1888cd9 100644
--- a/r2r/Cargo.toml
+++ b/r2r/Cargo.toml
@@ -57,3 +57,7 @@ features = ["doc-only"]
 [[bench]]
 name = "deserialization"
 harness = false
+
+[[example]]
+name = "timer_sim_time"
+required-features = ["sim-time"]
diff --git a/r2r/examples/sim_time_publisher.rs b/r2r/examples/sim_time_publisher.rs
new file mode 100644
index 0000000..c0ec9e4
--- /dev/null
+++ b/r2r/examples/sim_time_publisher.rs
@@ -0,0 +1,30 @@
+use r2r::rosgraph_msgs::msg;
+use r2r::ClockType::SystemTime;
+use r2r::{Clock, QosProfile};
+use std::time::Duration;
+
+/// Simple publisher publishing time starting at time 0 every `SENDING_PERIOD`
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let ctx = r2r::Context::create()?;
+    let mut node = r2r::Node::create(ctx, "clock_publisher", "")?;
+    let qos = QosProfile::default().keep_last(1);
+    let publisher = node.create_publisher("/clock", qos)?;
+
+    const SENDING_PERIOD: Duration = Duration::from_millis(100);
+    const SIM_TIME_MULTIPLIER: f64 = 0.1;
+
+    let mut clock = Clock::create(SystemTime)?;
+    let zero_time = clock.get_now()?;
+    let mut msg = msg::Clock::default();
+
+    loop {
+        let time_diff = clock.get_now()? - zero_time;
+        let time = time_diff.mul_f64(SIM_TIME_MULTIPLIER);
+        msg.clock = Clock::to_builtin_time(&time);
+
+        publisher.publish(&msg)?;
+        println!("Publishing time {}.{:9} s", time.as_secs(), time.subsec_nanos());
+
+        std::thread::sleep(SENDING_PERIOD);
+    }
+}
diff --git a/r2r/examples/timer_sim_time.rs b/r2r/examples/timer_sim_time.rs
new file mode 100644
index 0000000..fa87e48
--- /dev/null
+++ b/r2r/examples/timer_sim_time.rs
@@ -0,0 +1,82 @@
+use futures::executor::LocalPool;
+use futures::task::LocalSpawnExt;
+
+use r2r::{Clock, ClockType};
+use std::cell::RefCell;
+use std::rc::Rc;
+use std::sync::{Arc, Mutex};
+
+async fn timer_task(
+    mut t: r2r::Timer, ros_clock: Arc<Mutex<Clock>>, mut system_clock: Clock,
+) -> Result<(), Box<dyn std::error::Error>> {
+    let mut iteration: i32 = 0;
+    loop {
+        let elapsed = t.tick().await?;
+
+        let ros_time = ros_clock.lock().unwrap().get_now()?;
+        let system_time = system_clock.get_now()?;
+
+        println!("Timer called ({}), {}us since last call", iteration, elapsed.as_micros());
+        println!(
+            "\tcurrent time ros={:.3}s, system={:.3}s",
+            ros_time.as_secs_f64(),
+            system_time.as_secs_f64()
+        );
+
+        iteration += 1;
+        if iteration == 10 {
+            break;
+        }
+    }
+    Ok(())
+}
+
+/// Publication of time can be done either using the example `sim_time_publisher`
+/// or with `ros2 bag play --clock <clock_frequency> <the_bag>`
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let ctx = r2r::Context::create()?;
+    let mut node = r2r::Node::create(ctx, "testnode", "")?;
+
+    let mut pool = LocalPool::new();
+    let spawner = pool.spawner();
+
+    // Simulated time can be enabled by registering parameter handler
+    // and starting the program this ros param 'use_sim_time:=true'
+    // ```shell
+    // cargo run --features=sim-time --example=timer_sim_time -- --ros-args -p use_sim_time:=true
+    // ```
+    // this does not work if the parameter is changed during the runtime
+    let (paramater_handler, _parameter_events) = node.make_parameter_handler()?;
+    spawner.spawn_local(paramater_handler)?;
+
+    // or simulated time can be enabled/disabled directly by calling these functions:
+    // let time_source = node.get_time_source();
+    // time_source.enable_sim_time(&mut node)?;
+    // time_source.disable_sim_time();
+
+    // Note: Wall timer does not use sim time
+    let timer = node.create_timer(std::time::Duration::from_millis(1000))?;
+
+    let is_done = Rc::new(RefCell::new(false));
+
+    let task_is_done = is_done.clone();
+    let ros_clock = node.get_ros_clock();
+    let system_clock = Clock::create(ClockType::SystemTime)?;
+    spawner.spawn_local(async move {
+        match timer_task(timer, ros_clock, system_clock).await {
+            Ok(()) => {
+                *task_is_done.borrow_mut() = true;
+                println!("exiting");
+            }
+            Err(e) => println!("error: {}", e),
+        }
+    })?;
+
+    while !*is_done.borrow() {
+        node.spin_once(std::time::Duration::from_millis(100));
+
+        pool.run_until_stalled();
+    }
+
+    Ok(())
+}
diff --git a/r2r/src/time_source.rs b/r2r/src/time_source.rs
index e1500e9..32f346d 100644
--- a/r2r/src/time_source.rs
+++ b/r2r/src/time_source.rs
@@ -86,6 +86,8 @@ impl TimeSource {
     /// Enables usage of simulated time
     ///
     /// Simulated time is provided on topic `"/clock"` in the message [rosgraph_msgs::msg::Clock].
+    ///
+    /// See example: sim_time_publisher.rs
     pub fn enable_sim_time(&self, node: &mut Node) -> Result<()> {
         let mut inner = self.inner.lock().unwrap();
         if inner.simulated_time_enabled {