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; } }