任务执行

2026-05-28 10:05:40 +08:00
parent fcf5649d8c
commit 6ec778dc6d
12 changed files with 2681 additions and 0 deletions
@@ -0,0 +1,67 @@
+#!/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()