670 lines
24 KiB
Python
670 lines
24 KiB
Python
import time
|
|
from typing import Callable, Iterable
|
|
|
|
import cv2
|
|
import numpy as np
|
|
from aare.common.autofocus_tools import focus_measure_edges
|
|
from aare.common.beamline import mx_beamline
|
|
from aare.common.coordinate import Coordinate, SmargonCoordinate
|
|
from aare.common.models import AutofocusSettings
|
|
from aare.daq.config import BeamlineConfig
|
|
from aare.daq.daq import AareDAQ
|
|
from aare.daq.devices import BeamlineDevices
|
|
|
|
def make_circular_mask(shape_hw: tuple[int, int], center_x: float, center_y: float, radius: float) -> np.ndarray:
|
|
h, w = int(shape_hw[0]), int(shape_hw[1])
|
|
y, x = np.ogrid[:h, :w]
|
|
return (x - float(center_x)) ** 2 + (y - float(center_y)) ** 2 <= float(radius) ** 2
|
|
|
|
def _parabola_vertex(x1, y1, x2, y2, x3, y3) -> float | None:
|
|
# Fit parabola through 3 points; return vertex x if it's a maximum.
|
|
denom = (x1 - x2) * (x1 - x3) * (x2 - x3)
|
|
if abs(denom) < 1e-15:
|
|
return None
|
|
a = (x3 * (y2 - y1) + x2 * (y1 - y3) + x1 * (y3 - y2)) / denom
|
|
b = (x3**2 * (y1 - y2) + x2**2 * (y3 - y1) + x1**2 * (y2 - y3)) / denom
|
|
if a >= 0:
|
|
return None
|
|
return float(-b / (2 * a))
|
|
|
|
class AutofocusController:
|
|
"""
|
|
Fast autofocus: bracket -> ternary -> optional parabola.
|
|
|
|
You inject:
|
|
- get_gray_image(): np.ndarray (2D)
|
|
- get_frame_id(): int (UniqueId) OR None
|
|
- move_to(z): move stage to requested z (units are up to you)
|
|
- wait_for_stop(): block until motion ends
|
|
|
|
The key speed/robustness trick is waiting for a *new frame id* after motion.
|
|
"""
|
|
def __init__(
|
|
self,
|
|
*,
|
|
get_gray_image,
|
|
focus_measure,
|
|
move_to,
|
|
wait_for_stop,
|
|
get_frame_id=None,
|
|
fps: float = 25.0,
|
|
):
|
|
self.get_gray_image = get_gray_image
|
|
self.focus_measure = focus_measure
|
|
self.move_to = move_to
|
|
self.wait_for_stop = wait_for_stop
|
|
self.get_frame_id = get_frame_id
|
|
self.fps = float(fps)
|
|
|
|
self._uid_stuck_count = 0
|
|
self._uid_stuck_disable_after = 3
|
|
|
|
def _wait_new_frames(self, frames: int = 1, timeout_s: float = 0.12) -> bool:
|
|
"""
|
|
Wait for new frames by UniqueId.
|
|
If uid appears stuck (common in standalone tests if acquisition isn't running),
|
|
quickly fall back to a short sleep so autofocus stays fast.
|
|
"""
|
|
if self.get_frame_id is None or self._uid_stuck_count >= self._uid_stuck_disable_after:
|
|
time.sleep(max(0.0, float(frames)) / max(1e-6, self.fps))
|
|
return True
|
|
start = int(self.get_frame_id())
|
|
print(f"Waiting for {frames} frames (uid={start})...")
|
|
target = start + int(frames)
|
|
deadline = time.perf_counter() + float(timeout_s)
|
|
|
|
while time.perf_counter() < deadline:
|
|
if int(self.get_frame_id()) >= target:
|
|
self._uid_stuck_count = 0
|
|
print(f"Acquired {frames} frames (uid={target}, start={start})")
|
|
return True
|
|
time.sleep(0.001)
|
|
|
|
# uid didn't advance in time -> count as "stuck" and fall back
|
|
print(f"UID stuck for {timeout_s} s, falling back to sleep...")
|
|
self._uid_stuck_count += 1
|
|
time.sleep(1.0 / max(1e-6, self.fps))
|
|
return False
|
|
|
|
def _score_at(self, z, mask: np.ndarray | None, robust_frames: int) -> float:
|
|
st = time.perf_counter()
|
|
self.move_to(z)
|
|
print(f"move_to command to z={z:.2f} (t={time.perf_counter() - st:.5f} s)")
|
|
st = time.perf_counter()
|
|
self.wait_for_stop()
|
|
print(f"wait_for_stop command (t={time.perf_counter() - st:.5f} s)")
|
|
|
|
# Ensure next image is not a stale buffer
|
|
print("Waiting for new frame...")
|
|
st = time.perf_counter()
|
|
self._wait_new_frames(frames=1, timeout_s=0.4)
|
|
print(f"Acquired new frame (t={time.perf_counter() - st:.5f} s)")
|
|
st = time.perf_counter()
|
|
if robust_frames <= 1:
|
|
gray = self.get_gray_image()
|
|
print(f"got grey image (t={time.perf_counter() - st:.5f} s)")
|
|
return float(self.focus_measure(gray, mask))
|
|
|
|
vals: list[float] = []
|
|
for _ in range(int(robust_frames)):
|
|
gray = self.get_gray_image()
|
|
vals.append(float(self.focus_measure(gray, mask)))
|
|
self._wait_new_frames(frames=1, timeout_s=0.4)
|
|
print(f'Got values after {time.perf_counter() - st:.5f} s: {vals}')
|
|
return float(np.median(np.asarray(vals, dtype=np.float64)))
|
|
|
|
def run_once(
|
|
self,
|
|
*,
|
|
z0: float,
|
|
z_range: float,
|
|
mask: np.ndarray | None = None,
|
|
robust_frames: int = 1,
|
|
ternary_iters: int = 4,
|
|
do_parabola: bool = True,
|
|
edge_stop: bool = True,
|
|
flat_rel_tol: float = 0.03,
|
|
) -> tuple[float, float]:
|
|
"""
|
|
Returns (best_z, best_focus).
|
|
|
|
edge_stop:
|
|
If True and the best bracket point is at ±0.5*z_range, stop early.
|
|
(Means the peak is likely outside the search window.)
|
|
|
|
flat_rel_tol:
|
|
If (max-min)/max is below this, treat focus curve as flat and stop early.
|
|
"""
|
|
R = float(z_range)
|
|
|
|
# 1) 5-point bracket
|
|
zs = np.array(
|
|
[z0 - 0.5 * R, z0 - 0.25 * R, z0, z0 + 0.25 * R, z0 + 0.5 * R],
|
|
dtype=np.float64,
|
|
)
|
|
fs = np.array([self._score_at(float(z), mask, robust_frames) for z in zs], dtype=np.float64)
|
|
f0 = float(fs[2]) # z0
|
|
f_max = float(fs.max())
|
|
if f0 > 0 and (f_max / f0) < 1.05: # <5% improvement available
|
|
return float(zs[2]), f0
|
|
|
|
best_i = int(np.argmax(fs))
|
|
z_best = float(zs[best_i])
|
|
f_best = float(fs[best_i])
|
|
|
|
# Early exit if the curve is basically flat (no meaningful improvement)
|
|
f_min = float(fs.min())
|
|
if f_max > 0 and (f_max - f_min) / f_max < float(flat_rel_tol):
|
|
return z_best, f_best
|
|
|
|
# Early exit if best is at range edge: bracket does not contain a maximum
|
|
if edge_stop and (best_i == 0 or best_i == len(zs) - 1):
|
|
return z_best, f_best
|
|
|
|
# Local bracket for ternary search
|
|
iL = max(0, best_i - 1)
|
|
iR = min(len(zs) - 1, best_i + 1)
|
|
zL, zR = float(zs[iL]), float(zs[iR])
|
|
|
|
sampled: dict[float, float] = {float(zs[i]): float(fs[i]) for i in range(len(zs))}
|
|
|
|
if zL == zR:
|
|
return z_best, f_best
|
|
|
|
# 2) ternary search in local bracket (assumes unimodal-ish)
|
|
for _ in range(int(ternary_iters)):
|
|
a, b = (zL, zR) if zL < zR else (zR, zL)
|
|
z1 = a + (b - a) / 3.0
|
|
z2 = b - (b - a) / 3.0
|
|
|
|
if z1 not in sampled:
|
|
sampled[z1] = self._score_at(float(z1), mask, robust_frames)
|
|
if z2 not in sampled:
|
|
sampled[z2] = self._score_at(float(z2), mask, robust_frames)
|
|
|
|
if sampled[z1] < sampled[z2]:
|
|
zL = z1
|
|
else:
|
|
zR = z2
|
|
|
|
# 3) optional 3-point parabola around current best sample
|
|
if do_parabola and len(sampled) >= 3:
|
|
items = sorted(sampled.items(), key=lambda t: t[0])
|
|
zz = np.array([p[0] for p in items], dtype=np.float64)
|
|
ff = np.array([p[1] for p in items], dtype=np.float64)
|
|
k = int(np.argmax(ff))
|
|
|
|
if 0 < k < len(zz) - 1:
|
|
zv = _parabola_vertex(
|
|
float(zz[k - 1]), float(ff[k - 1]),
|
|
float(zz[k]), float(ff[k]),
|
|
float(zz[k + 1]), float(ff[k + 1]),
|
|
)
|
|
if zv is not None and float(zz[k - 1]) <= zv <= float(zz[k + 1]):
|
|
if zv not in sampled:
|
|
sampled[zv] = self._score_at(float(zv), mask, robust_frames)
|
|
|
|
z_best, f_best = max(sampled.items(), key=lambda t: t[1])
|
|
return float(z_best), float(f_best)
|
|
|
|
def __auto_focus(settings: AutofocusSettings) -> float:
|
|
"""
|
|
Fast autofocus on Smargon Z:
|
|
- bracket (5 points)
|
|
- ternary search (few iters)
|
|
- optional parabola refine
|
|
|
|
Returns:
|
|
Best Z offset in mm (beamline Z delta) relative to the starting position.
|
|
"""
|
|
geom = daq.sample_geometry
|
|
start_smargon = devs.smargon_pos
|
|
|
|
# ROI center: use provided, else use beam location (beam mark)
|
|
center_x = float(settings.center_x_pxl) if settings.center_x_pxl is not None else float(geom.beam_location_pxl.x)
|
|
center_y = float(settings.center_y_pxl) if settings.center_y_pxl is not None else float(geom.beam_location_pxl.y)
|
|
radius_pxl = float(settings.radius_pxl)
|
|
|
|
z_range_mm = float(settings.z_range_um) / 1000.0
|
|
z_steps = int(settings.z_steps)
|
|
|
|
# Build mask once (needs image shape)
|
|
first = daq.camera_image_gray
|
|
if first is None:
|
|
raise RuntimeError("Autofocus: no camera image available.")
|
|
if first.ndim != 2:
|
|
raise RuntimeError("Autofocus: expected grayscale image (2D).")
|
|
|
|
#mask = make_circular_mask(first.shape[:2], center_x=center_x, center_y=center_y, radius=radius_pxl)
|
|
|
|
height, width = first.shape
|
|
y, x = np.ogrid[:height, :width]
|
|
mask = (x - center_x) ** 2 + (y - center_y) ** 2 <= radius_pxl ** 2
|
|
|
|
def move_to_delta_z_mm(dz_mm: float) -> None:
|
|
# Apply relative motion in *beamline Z* via the geometry transform
|
|
sh_new = start_smargon.sh_mm + geom.smargon_nudge(Coordinate(z=float(dz_mm)))
|
|
target = SmargonCoordinate(
|
|
sh_mm=sh_new,
|
|
phi_deg=start_smargon.phi_deg,
|
|
chi_deg=start_smargon.chi_deg,
|
|
)
|
|
devs.smargon_pos = target
|
|
|
|
def wait_for_stop() -> None:
|
|
devs.smargon_wait(timeout=30)
|
|
|
|
def get_gray() -> np.ndarray:
|
|
img = daq.camera_image_gray
|
|
if img is None:
|
|
raise RuntimeError("Autofocus: failed to acquire image.")
|
|
return img
|
|
|
|
ctrl = StepwiseAutofocus(
|
|
get_gray_image=get_gray,
|
|
focus_measure=focus_measure_edges,
|
|
move_to=move_to_delta_z_mm,
|
|
wait_for_stop=wait_for_stop,
|
|
get_frame_id=devs.samcam_frame_id,
|
|
fps=25.0,
|
|
)
|
|
|
|
# Robustness vs speed:
|
|
# - 1 is fastest
|
|
# - 2 is more stable (median of 2 frames) and often still < 1 s total
|
|
robust_frames = 1
|
|
|
|
best_dz, best_f, zs, fs = ctrl.run(z0=0.0, z_range=z_range_mm, z_steps=z_steps, mask=mask,
|
|
refine=False, include_baseline=False)
|
|
|
|
# Move to the best position (controller ends at last probed z; ensure final is best)
|
|
move_to_delta_z_mm(best_dz)
|
|
wait_for_stop()
|
|
|
|
print(
|
|
f"Autofocus complete: best_dz={best_dz * 1000.0:.1f} um, focus={best_f:.2f}, "
|
|
f"roi_center=({center_x:.1f},{center_y:.1f}), r={radius_pxl:.1f}px"
|
|
)
|
|
|
|
return float(best_dz)
|
|
|
|
# def auto_focus(self, settings: AutofocusSettings) -> float:
|
|
# """
|
|
# Public autofocus method. Only allowed in SampleAlignment state.
|
|
# Returns best Z offset in mm (beamline Z delta) relative to start.
|
|
# """
|
|
# self.__cfg.set_busy(BeamlineStateEnum.SampleAlignment)
|
|
# try:
|
|
# best_dz_mm = self.__auto_focus(settings)
|
|
# self.__cfg.state_busy = False
|
|
# return best_dz_mm
|
|
# except Exception as e:
|
|
# logger.error(f"Autofocus failed: {e}")
|
|
# self.__cfg.state_busy = False
|
|
# raise
|
|
class StepwiseAutofocus:
|
|
def __init__(self, *, get_gray_image, focus_measure, move_to, wait_for_stop, get_frame_id=None, fps=25.0):
|
|
self.get_gray_image = get_gray_image
|
|
self.focus_measure = focus_measure
|
|
self.move_to = move_to
|
|
self.wait_for_stop = wait_for_stop
|
|
self.get_frame_id = get_frame_id
|
|
self.fps = float(fps)
|
|
|
|
def _wait_new_frame(self, timeout_s: float = 0.25) -> None:
|
|
if self.get_frame_id is None:
|
|
time.sleep(1.0 / max(1e-6, self.fps))
|
|
return
|
|
start = int(self.get_frame_id())
|
|
deadline = time.perf_counter() + float(timeout_s)
|
|
while time.perf_counter() < deadline:
|
|
if int(self.get_frame_id()) > start:
|
|
return
|
|
time.sleep(0.001)
|
|
# fallback: don't hang
|
|
time.sleep(1.0 / max(1e-6, self.fps))
|
|
|
|
def score_at(self, z: float, mask=None) -> float:
|
|
self.move_to(float(z))
|
|
self.wait_for_stop()
|
|
self._wait_new_frame(timeout_s=0.25)
|
|
gray = self.get_gray_image()
|
|
return float(self.focus_measure(gray, mask))
|
|
|
|
def run(
|
|
self,
|
|
*,
|
|
z0: float,
|
|
z_range: float,
|
|
z_steps: int,
|
|
mask=None,
|
|
refine: bool = False,
|
|
include_baseline: bool = False,
|
|
) -> tuple[float, float, np.ndarray, np.ndarray]:
|
|
"""
|
|
Returns:
|
|
(best_z, best_focus, z_positions, focus_values)
|
|
|
|
If include_baseline=False and refine=False, this will evaluate focus exactly `z_steps` times.
|
|
"""
|
|
z_steps = int(z_steps)
|
|
if z_steps < 3:
|
|
raise ValueError("z_steps must be >= 3 for a meaningful scan.")
|
|
|
|
if include_baseline:
|
|
_ = self.score_at(float(z0), mask=mask)
|
|
|
|
zs = np.linspace(z0 - 0.5 * float(z_range), z0 + 0.5 * float(z_range), z_steps, dtype=np.float64)
|
|
fs = np.empty_like(zs)
|
|
|
|
for i, z in enumerate(zs):
|
|
fs[i] = self.score_at(float(z), mask=mask)
|
|
|
|
best_i = int(np.argmax(fs))
|
|
best_z = float(zs[best_i])
|
|
best_f = float(fs[best_i])
|
|
|
|
if refine and 0 < best_i < (len(zs) - 1):
|
|
dz = float(zs[best_i + 1] - zs[best_i])
|
|
z_candidates = np.array([best_z - dz, best_z, best_z + dz], dtype=np.float64)
|
|
f_candidates = np.array([self.score_at(float(zc), mask=mask) for zc in z_candidates], dtype=np.float64)
|
|
j = int(np.argmax(f_candidates))
|
|
best_z = float(z_candidates[j])
|
|
best_f = float(f_candidates[j])
|
|
|
|
return best_z, best_f, zs, fs
|
|
|
|
def __auto_focus_with_aerotech(settings: AutofocusSettings) -> float:
|
|
"""
|
|
1) Fast focus scan on Aerotech GMZ (true focus axis)
|
|
2) Return GMZ to home position
|
|
3) Apply one Smargon move to preserve the focus (using a local Jacobian estimate)
|
|
|
|
Returns:
|
|
Smargon delta (in the same "beamline z command" units you use in geom.smargon_nudge(Coordinate(z=...))).
|
|
"""
|
|
geom = daq.sample_geometry
|
|
start_smargon = devs.smargon_pos
|
|
|
|
center_x = float(geom.beam_location_pxl.x)
|
|
center_y = float(geom.beam_location_pxl.y)
|
|
radius_pxl = float(settings.radius_pxl)
|
|
|
|
z_range_mm = float(settings.z_range_um) / 1000.0
|
|
z_steps = int(settings.z_steps)
|
|
|
|
def get_gray() -> np.ndarray:
|
|
"""
|
|
Match GUI pipeline:
|
|
- if Bayer: debayer -> RGB
|
|
- flip horizontally
|
|
- convert to gray (uint8)
|
|
"""
|
|
img = daq.camera_image # <-- NOTE: use raw, not camera_image_gray
|
|
if img is None:
|
|
raise RuntimeError("Autofocus: failed to acquire image.")
|
|
|
|
# If already grayscale
|
|
if img.ndim == 2:
|
|
bayer = img.astype(np.uint8, copy=False)
|
|
rgb = cv2.cvtColor(bayer, cv2.COLOR_BAYER_GB2RGB)
|
|
rgb = rgb[:, ::-1, :].copy()
|
|
gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY)
|
|
return gray
|
|
|
|
# If RGB-like
|
|
if img.ndim == 3 and img.shape[2] >= 3:
|
|
rgb = img[:, :, :3]
|
|
rgb = rgb[:, ::-1, :].copy()
|
|
if rgb.dtype != np.uint8:
|
|
rgb = np.clip(rgb, 0, 255).astype(np.uint8)
|
|
gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY)
|
|
return gray
|
|
|
|
raise RuntimeError(f"Autofocus: unexpected image shape {img.shape}")
|
|
|
|
|
|
first = get_gray()
|
|
print(first.shape[:2])
|
|
print(center_x, center_y, radius_pxl)
|
|
print((first.shape[1]-1) - center_x)
|
|
if first is None or first.ndim != 2:
|
|
raise RuntimeError("Autofocus: no grayscale image available.")
|
|
mask = make_circular_mask(first.shape[:2], center_x=center_x, center_y=center_y, radius=radius_pxl)
|
|
|
|
def score_focus() -> float:
|
|
gray = get_gray()
|
|
|
|
# Ensure we're comparing apples-to-apples in logs
|
|
g = gray
|
|
if g.dtype != np.uint8:
|
|
g_u8 = np.clip(g, 0, 255).astype(np.uint8)
|
|
else:
|
|
g_u8 = g
|
|
|
|
roi = g_u8[mask]
|
|
mean_dn = float(roi.mean()) if roi.size else 0.0
|
|
std_dn = float(roi.std()) if roi.size else 0.0
|
|
|
|
raw = float(focus_measure_edges(g_u8, mask))
|
|
|
|
# Normalize to reduce exposure/gain dependence (gradient energy scales ~ intensity^2)
|
|
norm = raw / ((mean_dn + 1e-6) ** 2)
|
|
|
|
print(f"AF: mean={mean_dn:.1f} std={std_dn:.1f} raw_focus={raw:.2f} norm_focus={norm:.6f}")
|
|
return norm
|
|
|
|
# ---------
|
|
# A) Aerotech GMZ scan (relative to current GMZ = "home" for this autofocus call)
|
|
# ---------
|
|
aero0 = devs.aerotech_pos
|
|
gmz0 = float(aero0.z)
|
|
|
|
gmz_offsets = np.linspace(-0.5 * z_range_mm, 0.5 * z_range_mm, z_steps, dtype=np.float64)
|
|
gmz_scores = []
|
|
|
|
for dz in gmz_offsets:
|
|
devs.aerotech.move_motor_linear("Z", gmz0 + float(dz), 10)
|
|
# wait 1 new frame after motion so we don't score an old buffer
|
|
start_uid = devs.samcam_frame_id()
|
|
t_deadline = time.perf_counter() + 0.25
|
|
while time.perf_counter() < t_deadline and devs.samcam_frame_id() == start_uid:
|
|
time.sleep(0.001)
|
|
gmz_scores.append(score_focus())
|
|
|
|
gmz_scores = np.asarray(gmz_scores, dtype=np.float64)
|
|
best_i = int(np.argmax(gmz_scores))
|
|
best_gmz_offset = float(gmz_offsets[best_i])
|
|
best_focus = float(gmz_scores[best_i])
|
|
|
|
# Move GMZ back to "home" (gmz0)
|
|
devs.aerotech.move_motor_absolute("Z", gmz0, 10000)
|
|
|
|
# If best was ~0 anyway, nothing to bake in
|
|
if abs(best_gmz_offset) < 1e-6:
|
|
print(f"Aerotech prefocus: best_gmz_offset≈0, focus={best_focus:.2f}")
|
|
return 0.0
|
|
|
|
# ---------
|
|
# B) Estimate local Jacobian: how Aerotech GMZ changes per unit Smargon beamline-z command
|
|
# We do two probe moves in the Smargon command space and measure GMZ readback.
|
|
# ---------
|
|
def move_smargon_beamline_dz(dz_mm: float) -> None:
|
|
sh_new = start_smargon.sh_mm + geom.smargon_nudge(Coordinate(z=float(dz_mm)))
|
|
target = SmargonCoordinate(
|
|
sh_mm=sh_new,
|
|
phi_deg=start_smargon.phi_deg,
|
|
chi_deg=start_smargon.chi_deg,
|
|
)
|
|
devs.smargon_pos = target
|
|
devs.smargon_wait(timeout=30)
|
|
|
|
move_smargon_beamline_dz(best_gmz_offset)
|
|
|
|
print(
|
|
f"Aerotech prefocus: best_gmz_offset={best_gmz_offset*1000} um, focus={best_focus:.2f} "
|
|
f"Smargon_start: {start_smargon.sh_mm} um, Smargon_end: {devs.smargon_pos.sh_mm} um"
|
|
)
|
|
return float(best_gmz_offset)
|
|
|
|
def focus_measure_laplacian(gray: np.ndarray, mask: np.ndarray | None = None) -> float:
|
|
"""
|
|
Fast focus metric: variance of Laplacian.
|
|
|
|
Notes:
|
|
- Works best on uint8 images.
|
|
- Use a mask/ROI to avoid scoring irrelevant background.
|
|
"""
|
|
if gray is None:
|
|
return 0.0
|
|
if gray.ndim != 2:
|
|
raise ValueError(f"Expected 2D grayscale image, got shape={gray.shape}")
|
|
|
|
g = gray
|
|
if g.dtype != np.uint8:
|
|
g = np.clip(g, 0, 255).astype(np.uint8)
|
|
|
|
if mask is not None:
|
|
roi = g[mask]
|
|
if roi.size < 64: # too few pixels -> unstable variance
|
|
return 0.0
|
|
# Laplacian needs 2D input; reshape ROI to a thin image is awkward.
|
|
# Better: compute Laplacian on full image and then mask the result.
|
|
lap = cv2.Laplacian(g, cv2.CV_64F, ksize=3)
|
|
v = float(lap[mask].var())
|
|
return v
|
|
|
|
lap = cv2.Laplacian(g, cv2.CV_64F, ksize=3)
|
|
return float(lap.var())
|
|
|
|
|
|
def _wait_for_new_uid(
|
|
get_frame_id: Callable[[], int] | None,
|
|
last_uid: int | None,
|
|
*,
|
|
frames: int = 1,
|
|
timeout_s: float = 0.30,
|
|
poll_s: float = 0.002,
|
|
fallback_sleep_s: float = 0.04,
|
|
) -> int | None:
|
|
"""
|
|
Wait until UniqueId advances by `frames`.
|
|
Returns the new uid (or last_uid if we couldn't observe advancement).
|
|
"""
|
|
if get_frame_id is None:
|
|
time.sleep(fallback_sleep_s)
|
|
return last_uid
|
|
|
|
try:
|
|
uid0 = int(get_frame_id()) if last_uid is None else int(last_uid)
|
|
except Exception:
|
|
time.sleep(fallback_sleep_s)
|
|
return last_uid
|
|
|
|
target = uid0 + int(frames)
|
|
deadline = time.perf_counter() + float(timeout_s)
|
|
|
|
while time.perf_counter() < deadline:
|
|
try:
|
|
uid = int(get_frame_id())
|
|
except Exception:
|
|
uid = uid0
|
|
|
|
if uid >= target:
|
|
return uid
|
|
|
|
time.sleep(poll_s)
|
|
|
|
# Timeout: don't hang autofocus; just do a small sleep to reduce stale-buffer chance.
|
|
time.sleep(fallback_sleep_s)
|
|
return uid0
|
|
|
|
|
|
def autofocus_gpt(
|
|
z_positions: Iterable[float],
|
|
move_stage_fn: Callable[[float], None],
|
|
*,
|
|
get_frame_id: Callable[[], int] | None = None,
|
|
wait_for_stop: Callable[[], None] | None = None,
|
|
mask: np.ndarray | None = None,
|
|
robust_frames: int = 1,
|
|
) -> tuple[float, list[tuple[float, float]]]:
|
|
"""
|
|
Simple autofocus scan with reliability improvements:
|
|
- waits for a new UniqueId after motion (avoids scoring stale frames)
|
|
- optional median-of-N scoring per z
|
|
"""
|
|
measures: list[tuple[float, float]] = []
|
|
last_uid: int | None = None
|
|
|
|
# Prime last_uid so the first point also waits for a "fresh" frame
|
|
if get_frame_id is not None:
|
|
try:
|
|
last_uid = int(get_frame_id())
|
|
except Exception:
|
|
last_uid = None
|
|
|
|
for z in z_positions:
|
|
move_stage_fn(float(z))
|
|
if wait_for_stop is not None:
|
|
wait_for_stop()
|
|
|
|
# Wait for camera to deliver a frame AFTER the move
|
|
last_uid = _wait_for_new_uid(get_frame_id, last_uid, frames=1, timeout_s=0.35)
|
|
|
|
if robust_frames <= 1:
|
|
img = daq.camera_image_gray
|
|
score = focus_measure_laplacian(img, mask=mask)
|
|
else:
|
|
vals: list[float] = []
|
|
for _ in range(int(robust_frames)):
|
|
img = daq.camera_image_gray
|
|
vals.append(focus_measure_laplacian(img, mask=mask))
|
|
last_uid = _wait_for_new_uid(get_frame_id, last_uid, frames=1, timeout_s=0.35)
|
|
score = float(np.median(np.asarray(vals, dtype=np.float64)))
|
|
|
|
measures.append((float(z), float(score)))
|
|
print(f"Z={z:.6f}, sharpness={score:.3f}")
|
|
|
|
best_z = max(measures, key=lambda x: x[1])[0]
|
|
return best_z, measures
|
|
|
|
# ---- Example z positions ----
|
|
coarse = np.linspace(-0.1, 0.1, 10) # 0 to 200 microns in 10µm steps
|
|
|
|
def move_stage(z):
|
|
# Insert your hardware code here:
|
|
print(z)
|
|
devs.aerotech.move_motor_absolute("Z", z, 1000)
|
|
#devs.aerotech.controller.read_status()
|
|
# e.g. serial.write(f"MOVE Z {z}")
|
|
|
|
|
|
def get_frame_id():
|
|
return int(devs.samcam_frame_id())
|
|
|
|
if __name__ == "__main__":
|
|
devs = BeamlineDevices(mx_beamline())
|
|
cfg = BeamlineConfig(mx_beamline())
|
|
daq = AareDAQ(cfg, bl=mx_beamline())
|
|
zoom = devs.zoom
|
|
beam_center = cfg.get_beam_mark(zoom)
|
|
settings = AutofocusSettings(center_x_pxl=beam_center[0], center_y_pxl=beam_center[1],
|
|
radius_pxl=30, z_range_um=400, z_steps=40)
|
|
st = time.perf_counter()
|
|
best_z, curve = autofocus_gpt(coarse, move_stage, get_frame_id=get_frame_id)
|
|
move_stage(0)
|
|
#move_stage(best_z)
|
|
geom = daq.sample_geometry
|
|
start_smargon = devs.smargon_pos
|
|
|
|
sh_new = start_smargon.sh_mm + geom.smargon_nudge(Coordinate(z=float(best_z)))
|
|
target = SmargonCoordinate(
|
|
sh_mm=sh_new,
|
|
phi_deg=start_smargon.phi_deg,
|
|
chi_deg=start_smargon.chi_deg,
|
|
)
|
|
devs.smargon_pos = target
|
|
devs.smargon_wait(timeout=30)
|
|
print("Best focus at:", best_z)
|
|
print(f"Total time: {time.perf_counter() - st:.5f} s") |