89 lines
3.5 KiB
Python
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))
|