From c48e483d6064d25d44eeea5791922d116015c262 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Sat, 11 Apr 2026 13:38:13 +0200
Subject: [PATCH] Use header time instead of actual time for determining time
 passed

---
 toid_bt/include/toid_bt/plugins/in_position_decorator.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp
index ea95d7f..dc5e81f 100644
--- a/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp
+++ b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp
@@ -51,13 +51,13 @@ public:
 
         if(checkIfInPose(pose)) {
             if(checkIfPosesAreClose(last_pos, pose)) {
-                if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
+                if(rclcpp::Time(pose.header.stamp) - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
                     haltChild();
                     return BT::NodeStatus::SUCCESS;
                 }
             } else {
                 last_pos = pose;
-                last_pos_change = clock.now();
+                last_pos_change = pose.header.stamp;
             }
         }