#!/usr/bin/env python3
"""系统时钟发布器。

向 /clock 持续发布系统时间，配合 Nav2 的 use_sim_time 配置，避免 AGV
节点、雷达节点和导航栈之间出现时间源不一致。
"""
import os
import sys

import rclpy
from rclpy.node import Node
from rosgraph_msgs.msg import Clock


LOCKFILE = "/tmp/clock_publisher.lock"
PUBLISH_INTERVAL_SECONDS = 0.01
LOG_INTERVAL_COUNT = 1000


def ensure_single_instance(lockfile: str) -> None:
    if os.path.exists(lockfile):
        with open(lockfile) as f:
            old_pid = int(f.read().strip())
        try:
            os.kill(old_pid, 0)
            print(f"Another clock_publisher running PID {old_pid}, exit.", file=sys.stderr)
            sys.exit(1)
        except (OSError, ProcessLookupError):
            print(f"Stale lock removed (PID {old_pid} dead)", file=sys.stderr)

    with open(lockfile, "w") as f:
        f.write(str(os.getpid()))


class SystemClockPublisher(Node):
    def __init__(self):
        super().__init__("system_clock_publisher")
        self.publisher = self.create_publisher(Clock, "/clock", 10)
        self.timer = self.create_timer(PUBLISH_INTERVAL_SECONDS, self.publish_clock)
        self.count = 0
        self.get_logger().info(f"Clock publisher PID={os.getpid()}, publishing at 100Hz")

    def publish_clock(self):
        message = Clock()
        message.clock = self.get_clock().now().to_msg()
        self.publisher.publish(message)
        self.count += 1
        if self.count % LOG_INTERVAL_COUNT == 0:
            self.get_logger().info(f"Published {self.count} clock messages")


def main():
    ensure_single_instance(LOCKFILE)
    rclpy.init(args=sys.argv[1:])
    node = SystemClockPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
        if os.path.exists(LOCKFILE):
            os.remove(LOCKFILE)


if __name__ == "__main__":
    main()