#!/usr/bin/env python3
"""修复激光雷达时间戳偏移的修正器 v5"""
import os, sys, rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from builtin_interfaces.msg import Time

LOCKFILE = "/tmp/scan_fixer.lock"

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 fixer 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()))

def main():
    rclpy.init(args=sys.argv[1:])
    node = Node('scan_timestamp_fixer')
    offset = 2.0
    pub = node.create_publisher(LaserScan, '/scan_corrected', 10)
    count = [0]

    def cb(msg: LaserScan):
        count[0] += 1
        s, ns = msg.header.stamp.sec, msg.header.stamp.nanosec
        s2 = s - int(offset)
        ns2 = ns - int((offset % 1) * 1e9)
        if ns2 < 0:
            ns2 += 1000000000
            s2 -= 1
        out = LaserScan()
        out.header.frame_id = msg.header.frame_id
        out.header.stamp = Time(sec=s2, nanosec=ns2)
        out.angle_min = msg.angle_min
        out.angle_max = msg.angle_max
        out.angle_increment = msg.angle_increment
        out.time_increment = msg.time_increment
        out.scan_time = msg.scan_time
        out.range_min = msg.range_min
        out.range_max = msg.range_max
        out.ranges = msg.ranges
        out.intensities = msg.intensities
        pub.publish(out)
        if count[0] % 200 == 0:
            node.get_logger().info(f'#{count[0]} /scan={s} -> /scan_corrected={s2}')

    node.create_subscription(LaserScan, '/scan', cb, 10)
    node.get_logger().info(f'Fixer PID={os.getpid()}, offset={offset}s')

    try:
        while rclpy.ok():
            rclpy.spin_once(node, timeout_sec=0.5)
    finally:
        node.destroy_node()
        rclpy.shutdown()
        if os.path.exists(LOCKFILE):
            os.unlink(LOCKFILE)

if __name__ == '__main__':
    main()