From d7de5debdaadf8cbf1d40566f112296c34c984d2 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Tue, 9 Dec 2025 22:34:32 +0100
Subject: [PATCH] Use realtime publisher for publishing the clock

---
 .../mujoco_system_interface.hpp                   |  2 ++
 src/mujoco_system_interface.cpp                   | 15 ++++++++++-----
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/include/mujoco_ros2_simulation/mujoco_system_interface.hpp b/include/mujoco_ros2_simulation/mujoco_system_interface.hpp
index d6fe217..c806811 100644
--- a/include/mujoco_ros2_simulation/mujoco_system_interface.hpp
+++ b/include/mujoco_ros2_simulation/mujoco_system_interface.hpp
@@ -33,6 +33,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
 #include <rclcpp_lifecycle/state.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 
 #include <mujoco/mujoco.h>
@@ -218,6 +219,7 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
 
   // Primary clock publisher for the world
   std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_publisher_;
+  realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_realtime_publisher_;
 
   // Containers for RGB-D cameras
   std::unique_ptr<MujocoCameras> cameras_;
diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp
index cedf0df..2799ec7 100644
--- a/src/mujoco_system_interface.cpp
+++ b/src/mujoco_system_interface.cpp
@@ -703,6 +703,8 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
 
   // Time publisher will be pushed from the physics_thread_
   clock_publisher_ = mujoco_node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
+  clock_realtime_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>>(clock_publisher_);
 
   // Ready cameras
   RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Initializing cameras...");
@@ -1639,8 +1641,7 @@ void MujocoSystemInterface::PhysicsLoop()
   while (!sim_->exitrequest.load())
   {
     // sleep for 1 ms or yield, to let main thread run
-    //  yield results in busy wait - which has better timing but kills battery
-    //  life
+    //  yield results in busy wait - which has better timing but kills battery life
     if (sim_->run && sim_->busywait)
     {
       std::this_thread::yield();
@@ -1715,8 +1716,9 @@ void MujocoSystemInterface::PhysicsLoop()
             double refreshTime = kSimRefreshFraction / sim_->refresh_rate;
 
             // step while sim lags behind cpu and within refreshTime.
-            while (Seconds((mj_data_->time - syncSim) * speedFactor) < mj::Simulate::Clock::now() - syncCPU &&
-                   mj::Simulate::Clock::now() - startCPU < Seconds(refreshTime))
+            auto currentCPU = mj::Simulate::Clock::now();
+            while (Seconds((mj_data_->time - syncSim) * speedFactor) < currentCPU - syncCPU &&
+                   currentCPU - startCPU < Seconds(refreshTime))
             {
               // measure slowdown before first step
               if (!measured && elapsedSim)
@@ -1749,6 +1751,9 @@ void MujocoSystemInterface::PhysicsLoop()
               {
                 break;
               }
+
+              // Update current CPU time for next iteration
+              currentCPU = mj::Simulate::Clock::now();
             }
           }
 
@@ -1788,7 +1793,7 @@ void MujocoSystemInterface::publish_clock()
 
   rosgraph_msgs::msg::Clock sim_time_msg;
   sim_time_msg.clock = sim_time_ros;
-  clock_publisher_->publish(sim_time_msg);
+  clock_realtime_publisher_->try_publish(sim_time_msg);
 }
 
 void MujocoSystemInterface::get_model(mjModel*& dest)