jumpback_detector.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rosgraph_msgs.msg import Clock
from rclpy.qos import QoSProfile, QoSReliabilityPolicy


class JumpbackDetector(Node):
    def __init__(self):
        super().__init__('jumpback_detector')
        self.clock = None

        qos_profile = QoSProfile(depth=1)
        qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
        self._sub_clock = self.create_subscription(
            Clock, '/clock', self._on_clock, qos_profile)

    def _on_clock(self, msg):
        self.clock = msg.clock


if __name__ == '__main__':
    rclpy.init()
    node = JumpbackDetector()
    previous_time = None
    while True:
        rclpy.spin_once(node, timeout_sec=2)
        current_time = node.clock.sec + node.clock.nanosec / 1000000000.
        if previous_time is not None:
            if current_time < previous_time:
                print("Jumpback {} -> {}".format(previous_time, current_time))
        previous_time = current_time