FLIR BLACKFLY S + YOLO-WORLD TO FANUC ROBOT
flir1c3d
Code
23,529 characters
"""
================================================================================
FLIR BLACKFLY S + YOLO-WORLD TO FANUC ROBOT PIPELINE
================================================================================
REQUIRED MODULES & PURPOSE:
--------------------------------------------------------------------------------
• PySpin (FLIR Spinnaker SDK):
Handles USB3 camera hardware control, frame acquisition, continuous auto-exposure,
auto-gain, frame rate throttling (30 FPS), and live sensor thermal monitoring.
• ultralytics (YOLOWorld):
Executes real-time zero-shot open-vocabulary object detection (yolov8s-worldv2.pt)
to classify target objects and return pixel bounding boxes without manual model training.
• torch (PyTorch):
Backend neural network runtime powering the YOLO-World detection model.
• cv2 (OpenCV):
Image processing (CLAHE contrast optimization, grayscale-to-BGR color mapping),
rendering the live GUI window, bounding boxes, crosshairs, and mouse click capture.
• numpy:
Converts Spinnaker raw buffer arrays into structured matrices compatible with OpenCV/Torch.
• pycomm3 (CIPDriver):
Sends explicit CIP Ethernet/IP messages (Class Code 0x6B) directly to the FANUC
R-30iB controller to write integer coordinates straight into Numeric Registers (R[]).
• struct:
Packs integer data into raw little-endian binary bytes (<i) required by CIP payloads.
• os, time, warnings:
Environment variable configuration, OpenMP thread safety, heartbeat timers,
thermal poll throttling, and warning suppression.
--------------------------------------------------------------------------------
REGISTER MAPPING (CONFIGURABLE VIA 'FANUC_REG_CONFIG'):
--------------------------------------------------------------------------------
• R[30]: VISION_X -> Target X coordinate in millimeters (Integer mm)
• R[31]: VISION_Y -> Target Y coordinate in millimeters (Integer mm)
• R[32]: VISION_R -> Target tool yaw/rotation in degrees (currently always 0 —
YOLO-World gives axis-aligned boxes with no orientation info;
real rotation needs a rotated-box detector or a PCA pass over
a mask once this moves to a trained model)
• R[33]: VISION_CLASS -> Object classification ID (1-based index matching prompts)
• R[34]: VISION_HB -> Watchdog heartbeat counter (cycles 0-9999). Only increments
when X/Y/R/CLASS all wrote successfully — treat "heartbeat
changed" as your signal that the other four registers are
a consistent, complete set. If your TP program doesn't
already gate on this, it should.
================================================================================
"""
import os
import struct
import time
import warnings
# Suppress PyTorch/Ultralytics serialization and future warnings
warnings.filterwarnings("ignore", category=FutureWarning)
warnings.filterwarnings("ignore", category=UserWarning)
os.environ["KMP_DUPLICATE_LIB_OK"] = "TRUE"
# Ensure PyTorch imports first to avoid Windows DLL conflicts
import torch
torch.set_num_threads(os.cpu_count() or 4)
import cv2
import numpy as np
import PySpin
from pycomm3 import CIPDriver
from ultralytics import YOLOWorld
# ==============================================================================
# CONFIGURATION & CONSTANTS
# ==============================================================================
# FANUC Controller Network Settings
ROBOT_IP = "192.168.20.101"
# FANUC CIP Object Definitions for Numeric Registers
CLASS_NUMREG = 0x6B
INSTANCE = 1
# ------------------------------------------------------------------------------
# FANUC REGISTER MAPPING CONFIGURATION
# Adjust these register numbers to match your Teach Pendant configuration
# ------------------------------------------------------------------------------
FANUC_REG_CONFIG = {
"VISION_X": 30, # R[30]: X coordinate in millimeters (Integer mm)
"VISION_Y": 31, # R[31]: Y coordinate in millimeters (Integer mm)
"VISION_R": 32, # R[32]: Rotation / Yaw angle in degrees
"VISION_CLASS": 33, # R[33]: Object Classification ID (1-based index)
"VISION_HB": 34, # R[34]: Watchdog Heartbeat tick (0-9999)
}
# Workspace Calibration (Millimeters per pixel at working distance)
MM_PER_PIX_X = 0.352
MM_PER_PIX_Y = 0.352
# Robot User Frame origin (mm) corresponding to image pixel (0, 0)
ORIGIN_ROBOT_X = 350.0
ORIGIN_ROBOT_Y = -150.0
# Fixed-Point Scale Factor:
# 1.0 = Direct Millimeters as Integers (e.g., 909 mm -> 909)
# 10.0 = Tenths of a Millimeter (e.g., 909.2 mm -> 9092, divide by 10.0 on TP)
SCALE_FACTOR = 1.0
# ------------------------------------------------------------------------------
# WORKSPACE SAFETY ENVELOPE (robot User Frame, millimeters)
# PLACEHOLDER VALUES — set these to your actual safe reachable envelope before
# this check means anything. A wrong-but-present bounds check is worse than
# no check at all, because it looks like protection that isn't there.
# ------------------------------------------------------------------------------
WORKSPACE_MIN_X = 150.0
WORKSPACE_MAX_X = 550.0
WORKSPACE_MIN_Y = -350.0
WORKSPACE_MAX_Y = 50.0
# CIP write retry policy — network hiccups on an industrial EtherNet/IP segment
# are common enough that one retry before giving up is worth the latency cost.
CIP_WRITE_RETRIES = 1
CIP_RETRY_DELAY_S = 0.05
# Camera Thermal Protection Thresholds (°C)
TEMP_WARN_THRESHOLD = 65.0
TEMP_SHUTDOWN_THRESHOLD = 72.0
# Inference & Streaming Rate
TARGET_CAMERA_FPS = 30.0
INFERENCE_INTERVAL = 3 # Run YOLO inference every 3rd frame
# Global UI state for click-to-send
click_queue = []
selected_target = None
# ==============================================================================
# FANUC CIP CLIENT
# ==============================================================================
class FanucCipClient:
"""Manages explicit CIP messaging to write directly to FANUC Numeric Registers."""
def __init__(self, host: str):
self.host = host
self.client = None
def connect(self) -> bool:
"""Initializes the CIP session."""
try:
self.disconnect()
self.client = CIPDriver(self.host)
self.client.open()
print(f"[CIP] Connected to FANUC controller at {self.host}")
return True
except Exception as ex:
print(f"[CIP ERR] Failed to connect: {ex}")
self.client = None
return False
def write_register_int(self, reg_num: int, value: int) -> bool:
"""Writes a 32-bit signed integer directly into R[reg_num], with a short
retry on failure before giving up (see CIP_WRITE_RETRIES)."""
payload = struct.pack("<i", int(value))
for attempt in range(CIP_WRITE_RETRIES + 1):
if not self.client or not self.client.connected:
if not self.connect():
if attempt < CIP_WRITE_RETRIES:
time.sleep(CIP_RETRY_DELAY_S)
continue
return False
resp = self.client.generic_message(
service=0x10, # Set_Attribute_Single
class_code=CLASS_NUMREG,
instance=INSTANCE,
attribute=reg_num,
request_data=payload,
)
if not resp.error:
return True
print(f"[CIP ERR] Write to R[{reg_num}] failed (attempt {attempt + 1}): {resp.error}")
if attempt < CIP_WRITE_RETRIES:
time.sleep(CIP_RETRY_DELAY_S)
return False
def disconnect(self):
"""Closes the CIP driver cleanly."""
if self.client:
try:
self.client.close()
except Exception:
pass
self.client = None
robot_client = FanucCipClient(ROBOT_IP)
def is_within_workspace(robot_x: float, robot_y: float) -> bool:
"""Checks a computed target against the configured safety envelope. This is
defense in depth, not a substitute for the TP program independently
validating positions before moving — a bug or miscalibration upstream of
this check (e.g. in the pixel->mm transform) could still compute a bad
coordinate; this just catches the case where that coordinate would land
clearly outside where the robot should ever be asked to go."""
return (
WORKSPACE_MIN_X <= robot_x <= WORKSPACE_MAX_X
and WORKSPACE_MIN_Y <= robot_y <= WORKSPACE_MAX_Y
)
def transmit_vision_telemetry(robot_x, robot_y, robot_r, class_id, label, conf) -> bool:
"""Pushes millimeter coordinates as clean integers into configured R[] registers.
Heartbeat only advances if X/Y/R/CLASS all wrote successfully — a failed or
partial transmission leaves the previous (fully consistent) register set in
place with its now-stale heartbeat, rather than pointing the TP program at
a mix of old and new values. Returns True only on a fully successful,
heartbeat-committed transmission.
"""
val_x = int(round(robot_x * SCALE_FACTOR))
val_y = int(round(robot_y * SCALE_FACTOR))
val_r = int(round(robot_r * SCALE_FACTOR))
reg_x = FANUC_REG_CONFIG["VISION_X"]
reg_y = FANUC_REG_CONFIG["VISION_Y"]
reg_r = FANUC_REG_CONFIG["VISION_R"]
reg_class = FANUC_REG_CONFIG["VISION_CLASS"]
reg_hb = FANUC_REG_CONFIG["VISION_HB"]
ok_x = robot_client.write_register_int(reg_x, val_x)
ok_y = robot_client.write_register_int(reg_y, val_y)
ok_r = robot_client.write_register_int(reg_r, val_r)
ok_c = robot_client.write_register_int(reg_class, class_id)
if not (ok_x and ok_y and ok_r and ok_c):
print(
f"[CIP FAIL] Incomplete write to {ROBOT_IP} — heartbeat NOT advanced, "
f"previous register set (if any) remains authoritative. "
f"(X:{ok_x} Y:{ok_y} R:{ok_r} CLASS:{ok_c})"
)
return False
heartbeat_tick = int(time.time() * 10) % 10000
ok_h = robot_client.write_register_int(reg_hb, heartbeat_tick)
if not ok_h:
print(
f"[CIP FAIL] Position registers wrote OK but heartbeat commit failed — "
f"TP program will not see this as fresh data until a future cycle "
f"successfully advances R[{reg_hb}]."
)
return False
print(
f"\n>>> [CIP TX OK] "
f"R[{reg_x}]: {val_x} mm | "
f"R[{reg_y}]: {val_y} mm | "
f"R[{reg_r}]: {val_r} deg | "
f"R[{reg_class}]: {class_id} ({label.upper()}) | "
f"R[{reg_hb}]: {heartbeat_tick}\n"
)
return True
# ==============================================================================
# MOUSE EVENT HANDLER
# ==============================================================================
def on_mouse_click(event, x, y, flags, param):
"""Captures left mouse clicks within the camera feed window."""
global click_queue
if event == cv2.EVENT_LBUTTONDOWN:
click_queue.append((x, y))
# ==============================================================================
# FLIR CAMERA CONFIGURATION
# ==============================================================================
def configure_camera(cam, target_fps=30.0):
"""Sets camera to Mono8, enables auto exposure/gain, and caps FPS."""
node_pixel_format = PySpin.CEnumerationPtr(cam.GetNodeMap().GetNode("PixelFormat"))
if PySpin.IsAvailable(node_pixel_format) and PySpin.IsWritable(node_pixel_format):
node_mono8 = node_pixel_format.GetEntryByName("Mono8")
if PySpin.IsAvailable(node_mono8) and PySpin.IsReadable(node_mono8):
node_pixel_format.SetIntValue(node_mono8.GetValue())
print("[CAMERA] Pixel format set to Mono8.")
if cam.ExposureAuto.GetAccessMode() == PySpin.RW:
cam.ExposureAuto.SetValue(PySpin.ExposureAuto_Continuous)
print("[CAMERA] Auto-exposure set to Continuous.")
if cam.GainAuto.GetAccessMode() == PySpin.RW:
cam.GainAuto.SetValue(PySpin.GainAuto_Continuous)
print("[CAMERA] Auto-gain set to Continuous.")
try:
fps_enable = cam.GetNodeMap().GetNode("AcquisitionFrameRateEnable")
if not PySpin.IsAvailable(fps_enable):
fps_enable = cam.GetNodeMap().GetNode("AcquisitionFrameRateEnabled")
node_fps_enable = PySpin.CBooleanPtr(fps_enable)
if PySpin.IsAvailable(node_fps_enable) and PySpin.IsWritable(node_fps_enable):
node_fps_enable.SetValue(True)
node_fps = PySpin.CFloatPtr(cam.GetNodeMap().GetNode("AcquisitionFrameRate"))
if PySpin.IsAvailable(node_fps) and PySpin.IsWritable(node_fps):
node_fps.SetValue(target_fps)
print(f"[CAMERA] Frame rate throttled to {target_fps} FPS.")
except PySpin.SpinnakerException as ex:
print(f"[CAMERA WARN] Frame rate cap bypassed: {ex}")
s_node_map = cam.GetTLStreamNodeMap()
handling_mode = PySpin.CEnumerationPtr(s_node_map.GetNode("StreamBufferHandlingMode"))
if PySpin.IsAvailable(handling_mode) and PySpin.IsWritable(handling_mode):
newest_only = handling_mode.GetEntryByName("NewestOnly")
if PySpin.IsAvailable(newest_only) and PySpin.IsReadable(newest_only):
handling_mode.SetIntValue(newest_only.GetValue())
print("[CAMERA] Stream buffer set to NewestOnly.")
def read_camera_temperature(cam):
"""Reads core sensor temperature in Celsius."""
try:
temp_node = PySpin.CFloatPtr(cam.GetNodeMap().GetNode("DeviceTemperature"))
if PySpin.IsAvailable(temp_node) and PySpin.IsReadable(temp_node):
return temp_node.GetValue()
except PySpin.SpinnakerException:
pass
return None
# ==============================================================================
# MAIN PIPELINE
# ==============================================================================
def main():
global click_queue, selected_target
print("[INIT] Opening EtherNet/IP CIP connection...")
if not robot_client.connect():
print("[WARN] Robot not reached. Will retry on target transmission.")
print("[INIT] Loading YOLO-World model...")
model = YOLOWorld("yolov8s-worldv2.pt")
target_classes = [
"computer mouse",
"screwdriver",
"precision screwdriver",
"hand tool",
"roll of tape",
"tape roll",
"electrical tape",
"black tape roll",
]
model.set_classes(target_classes)
print(f"[INIT] Active vocabulary prompts: {target_classes}")
system = PySpin.System.GetInstance()
cam_list = system.GetCameras()
cam = None # bound before the try block so `finally` never sees an unbound name
if cam_list.GetSize() == 0:
print("[ERROR] No FLIR Blackfly S camera found.")
cam_list.Clear()
system.ReleaseInstance()
return
clahe = cv2.createCLAHE(clipLimit=2.5, tileGridSize=(8, 8))
try:
cam = cam_list[0]
cam.Init()
configure_camera(cam, target_fps=TARGET_CAMERA_FPS)
cam.AcquisitionMode.SetValue(PySpin.AcquisitionMode_Continuous)
cam.BeginAcquisition()
win_name = "Blackfly S - CIP Direct Target Lock"
cv2.namedWindow(win_name, cv2.WINDOW_NORMAL)
cv2.resizeWindow(win_name, 960, 600)
cv2.setMouseCallback(win_name, on_mouse_click)
frame_count = 0
cached_boxes = []
current_temp = read_camera_temperature(cam) or 0.0
last_temp_check = time.time()
print("\n" + "=" * 70)
print("[READY] Registered Output Targets:")
for key, reg in FANUC_REG_CONFIG.items():
print(f" {key:<14} -> R[{reg}]")
print(f"[READY] Workspace envelope: X[{WORKSPACE_MIN_X}, {WORKSPACE_MAX_X}] "
f"Y[{WORKSPACE_MIN_Y}, {WORKSPACE_MAX_Y}] mm")
print("[READY] Click any bounding box to write coordinates straight to registers.")
print("[READY] Press 'q' in the camera window to exit.")
print("=" * 70 + "\n")
while True:
now = time.time()
try:
# 1. Thermal protection check (1 Hz)
if now - last_temp_check > 1.0:
temp_val = read_camera_temperature(cam)
if temp_val is not None:
current_temp = temp_val
if current_temp >= TEMP_SHUTDOWN_THRESHOLD:
print(f"\n[THERMAL CUTOFF] Temperature {current_temp:.1f}°C exceeded limit. Halting.")
break
last_temp_check = now
# 2. Retrieve frame buffer
image_result = cam.GetNextImage(1000)
if image_result.IsIncomplete():
image_result.Release()
continue
raw_mono = image_result.GetNDArray()
image_result.Release()
# 3. Contrast enhancement
enhanced_mono = clahe.apply(raw_mono)
frame_bgr = cv2.cvtColor(enhanced_mono, cv2.COLOR_GRAY2BGR)
# 4. YOLO Inference cycle
if frame_count % INFERENCE_INTERVAL == 0:
results = model.predict(frame_bgr, imgsz=480, conf=0.08, verbose=False)
cached_boxes = []
for box in results[0].boxes:
xyxy = box.xyxy[0].cpu().numpy().astype(int)
cls_id = int(box.cls[0].item())
label = target_classes[cls_id] if cls_id < len(target_classes) else "object"
conf_val = float(box.conf[0].item())
cached_boxes.append((xyxy, label, conf_val, cls_id))
frame_count += 1
# 5. Process click selections
while click_queue:
click_x, click_y = click_queue.pop(0)
matched = False
for (x1, y1, x2, y2), label, conf_val, cls_id in cached_boxes:
if x1 <= click_x <= x2 and y1 <= click_y <= y2:
cx = (x1 + x2) / 2.0
cy = (y1 + y2) / 2.0
robot_x = ORIGIN_ROBOT_X + (cx * MM_PER_PIX_X)
robot_y = ORIGIN_ROBOT_Y + (cy * MM_PER_PIX_Y)
robot_r = 0.0
if not is_within_workspace(robot_x, robot_y):
print(
f"[REJECTED] Target at X:{robot_x:.1f} Y:{robot_y:.1f} mm falls "
f"outside the configured workspace envelope — not transmitted."
)
matched = True
break
transmitted = transmit_vision_telemetry(
robot_x, robot_y, robot_r, cls_id + 1, label, conf_val
)
if transmitted:
selected_target = {
"box": (x1, y1, x2, y2),
"center": (cx, cy),
"x": robot_x,
"y": robot_y,
"r": robot_r,
"label": label,
"class_id": cls_id + 1,
"conf": conf_val,
}
# If the transmission failed, deliberately leave
# selected_target pointing at whatever was last
# successfully committed (or None) — the on-screen
# lock indicator should never claim a target that
# wasn't actually sent.
matched = True
break
if not matched:
print(f"[CLICK] ({click_x}, {click_y}) outside detected boxes.")
# 6. Render candidate boxes
for (x1, y1, x2, y2), label, conf_val, _ in cached_boxes:
box_color = (255, 60, 60)
thickness = 2
if selected_target and (x1, y1, x2, y2) == selected_target["box"]:
box_color = (0, 255, 0)
thickness = 3
cv2.rectangle(frame_bgr, (x1, y1), (x2, y2), box_color, thickness)
tag = f"{label} {conf_val:.2f}"
cv2.putText(frame_bgr, tag, (x1, max(22, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.55, box_color, 2)
# 7. Render target lock crosshair
if selected_target:
cx, cy = selected_target["center"]
cv2.drawMarker(frame_bgr, (int(cx), int(cy)), (0, 255, 0), cv2.MARKER_CROSS, 28, 2)
info_text = f"LOCKED -> X:{selected_target['x']:.1f} Y:{selected_target['y']:.1f} R:{selected_target['r']:.1f}"
cv2.putText(frame_bgr, info_text, (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2)
# 8. Render thermal status
temp_color = (0, 255, 0) if current_temp < TEMP_WARN_THRESHOLD else (0, 0, 255)
cv2.putText(
frame_bgr,
f"Temp: {current_temp:.1f}C",
(20, 40),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
temp_color,
2,
)
cv2.imshow(win_name, frame_bgr)
if cv2.waitKey(1) & 0xFF == ord("q"):
print("\n[STOP] Shutting down application...")
break
except PySpin.SpinnakerException as ex:
# Camera/acquisition-layer error on this specific frame — log
# and try to keep the session alive rather than tearing down
# the whole pipeline over one bad frame.
print(f"[SPINNAKER EXCEPTION] {ex}")
continue
except Exception as ex:
# Anything else (torch/ultralytics/OpenCV/etc.) — previously
# this would propagate uncaught and crash the whole session
# with no clear indication of what failed. Now it's logged
# and the loop continues; if it's a persistent/fatal
# condition it'll keep erroring visibly rather than dying
# silently mid-loop with the robot potentially still holding
# whatever the last-sent target was.
print(f"[UNEXPECTED ERROR] {type(ex).__name__}: {ex}")
continue
finally:
cv2.destroyAllWindows()
robot_client.disconnect()
if cam is not None:
try:
if cam.IsStreaming():
cam.EndAcquisition()
cam.DeInit()
except Exception:
pass
del cam
cam_list.Clear()
system.ReleaseInstance()
print("[CLEANUP] Camera stopped and CIP session closed.")
if __name__ == "__main__":
main()