ywb
67 lines
2.1 KiB
Python
67 lines
2.1 KiB
Python
#!/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() |