Files

89 lines
3.5 KiB
Python

#!/usr/bin/env python3
"""Start only the camera driver if needed; wait for fresh image and intrinsics."""
import os
from pathlib import Path
import signal
import subprocess
import sys
import time
import rclpy
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import CameraInfo, Image
from storage import stamp
ROOT = Path(__file__).resolve().parents[1]
IMAGE = '/camera/camera/color/image_raw'
INFO = '/camera/camera/color/camera_info'
def main():
rclpy.init()
node = rclpy.create_node('handeye_ensure_camera')
received = set()
node.create_subscription(Image, IMAGE, lambda m: received.add('image')
if m.data and m.encoding in ('rgb8', 'bgr8') else None,
qos_profile_sensor_data)
node.create_subscription(CameraInfo, INFO, lambda m: received.add('info')
if m.k[0] > 0 and m.k[4] > 0 else None,
qos_profile_sensor_data)
def wait_ready(seconds, process=None):
deadline = time.monotonic() + seconds
while time.monotonic() < deadline:
rclpy.spin_once(node, timeout_sec=.1)
if len(received) == 2:
return True
if process is not None and process.poll() is not None:
return False
return False
owned = None
ready = False
log_path = None
try:
if wait_ready(4):
ready = True
print('相机图像和内参已就绪,复用已有驱动。', file=sys.stderr)
return
running = (node.count_publishers(IMAGE) or node.count_publishers(INFO)
or ('camera', '/camera') in node.get_node_names_and_namespaces())
if running:
print('已有相机节点,等待消息,不重复启动。', file=sys.stderr)
else:
logs = ROOT / 'logs'
logs.mkdir(exist_ok=True)
log_path = logs / f'camera_driver_{stamp()}.log'
with log_path.open('w') as output:
# A separate session and closed FDs let the driver survive
# capture and prevent inheritance of run.sh's session lock.
owned = subprocess.Popen([
'ros2', 'launch', 'realsense2_camera', 'rs_launch.py',
'rgb_camera.color_profile:=848x480x30',
'depth_module.depth_profile:=848x480x30',
'align_depth.enable:=true',
], stdout=output, stderr=subprocess.STDOUT,
stdin=subprocess.DEVNULL, start_new_session=True, close_fds=True)
print(f'正在启动D405驱动,PID={owned.pid},日志:{log_path}', file=sys.stderr)
if not wait_ready(30, owned):
raise RuntimeError('相机图像/内参未就绪。检查USB连接、设备占用和相机驱动日志。'
+ (f' 日志:{log_path}' if log_path else ''))
ready = True
print('相机图像和内参已就绪,开始标定采集;驱动在采集结束后保持运行。', file=sys.stderr)
finally:
if owned is not None and not ready and owned.poll() is None:
os.killpg(owned.pid, signal.SIGINT)
try:
owned.wait(timeout=5)
except subprocess.TimeoutExpired:
os.killpg(owned.pid, signal.SIGTERM)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
try:
main()
except (RuntimeError, OSError) as error:
raise SystemExit(str(error))