2026-01-26 18:56:53 +09:00

436 lines
20 KiB
C#

/*
* Copyright (c) 2025 LG Electronics Inc.
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
*/
using Aurora.Api;
using VisionCalibrator;
using VisionCommon;
using System.Collections.Generic;
using VisionSequence.Camera;
using System.Linq;
using System;
namespace VisionSequence.Pattern {
/// <summary>
/// 카메라 그랩, 픽스처와 메인 패턴 검출 시퀀스 파라미터
/// </summary>
public class GrabFindWithFixtureSeqParams : GrabFindProcSeqParams {
/// <summary>
/// 생성자
/// </summary>
/// <param name="fovInfo">FOV 키 정보</param>
/// <param name="patternInfo">패턴 정보</param>
/// <param name="displayType">메인 화면 타입</param>
/// <param name="seqMode">시퀀스 모드</param>
public GrabFindWithFixtureSeqParams(
FovInfo fovInfo,
PatternInfo patternInfo,
eDisplayType displayType = eDisplayType.Main_1,
eSeqMode seqMode = eSeqMode.Auto) : base(fovInfo, patternInfo, displayType, seqMode) {
}
}
/// <summary>
/// 카메라 그랩, 픽스처와 메인 패턴 검출 시퀀스
/// </summary>
public class GrabFindWithFixtureSeq : SubSeqBase {
private readonly Logger _Logger;
private TactTimeChecker _TactTimeChecker;
private GrabFindWithFixtureSeqParams _Params;
private FovRecipe _FovRecipe;
private PatternAgentBase _PatternAgent;
private PatternInfo _FixturePatternInfo;
private Calibrator _Calibrator;
private CameraAgentBase _CameraAgent;
private ICameraDisplay _CameraDisplay;
private ImageManager _ImageManager = new ImageManager();
private string _SeqName;
// Get Context Key Values - Align에서 사용 할 때만 적용
private ProcParams _ProcParams;
#region Sequence Id
private const string SEQ_ID_INIT = "INIT";
private const string SEQ_ID_SET_GRAB_FIXTURE_SEQ_PARAMS = "SET_GRAB_FIXTURE_SEQ_PARAMS";
private const string SEQ_ID_GRAB_FIXTURE_SEQ = "GRAB_FIXTURE_SEQ";
private const string SEQ_ID_FIND_FIXTURE = "FIND_FIXTURE";
private const string SEQ_ID_CHECK_FIXTURE_FOUND = "CHECK_FIXTURE_FOUND";
private const string SEQ_ID_FIXTURE_FORCE_OK = "FIXTURE_FORCE_OK";
private const string SEQ_ID_CHECK_SAME_LIGHT = "CHECK_SAME_LIGHT";
private const string SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS = "SET_GRAB_MAIN_SEQ_PARAMS";
private const string SEQ_ID_GRAB_MAIN_SEQ = "GRAB_MAIN_SEQ";
private const string SEQ_ID_FIND_MAIN_WITH_FIXTURE = "FIND_MAIN_WITH_FIXTURE";
private const string SEQ_ID_CHECK_MAIN_FOUND = "CHECK_MAIN_FOUND";
private const string SEQ_ID_LIGHT_RETRY = "LIGHT_RETRY";
private const string SEQ_ID_MAIN_FORCE_OK = "MAIN_FORCE_OK";
private const string SEQ_ID_UPDATE_UI = "UPDATE_UI";
private const string SEQ_ID_SAVE_IMAGE = "SAVE_IMAGE";
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
#endregion
/// <summary>
/// 생성자
/// </summary>
public GrabFindWithFixtureSeq() {
_Logger = new Logger($"{GetType().Name}");
}
private void SetActions(SequenceBuilder sb) {
var grabSeqBuilder_Fixture = SubSeqManager.CreateSubSeqBuilder(new GrabSeq(), "GrabSeq_Fixture");
var grabSeqBuilder_Main = SubSeqManager.CreateSubSeqBuilder(new GrabSeq(), "GrabSeq_Main");
sb.AddAction(SEQ_ID_INIT, Init);
sb.AddAction(SEQ_ID_SET_GRAB_FIXTURE_SEQ_PARAMS, SetGrabFixtureSeqParams);
sb.AddSubsequence(SEQ_ID_GRAB_FIXTURE_SEQ, grabSeqBuilder_Fixture);
sb.AddAction(SEQ_ID_FIND_FIXTURE, FindFixture);
sb.AddDecision(SEQ_ID_CHECK_FIXTURE_FOUND, CheckFixtureFound);
sb.AddAction(SEQ_ID_FIXTURE_FORCE_OK, ForceOK);
sb.AddDecision(SEQ_ID_CHECK_SAME_LIGHT, CheckSameLight);
sb.AddAction(SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS, SetGrabMainSeqParams);
sb.AddSubsequence(SEQ_ID_GRAB_MAIN_SEQ, grabSeqBuilder_Main);
sb.AddAction(SEQ_ID_FIND_MAIN_WITH_FIXTURE, FindMainWithFixture);
sb.AddDecision(SEQ_ID_CHECK_MAIN_FOUND, CheckMainFound);
sb.AddDecision(SEQ_ID_LIGHT_RETRY, LightRetry);
sb.AddAction(SEQ_ID_MAIN_FORCE_OK, ForceOK);
sb.AddAction(SEQ_ID_UPDATE_UI, UpdateUI);
sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
}
public override SequenceBuilder Builder(string seqName) {
_SeqName = seqName;
_TactTimeChecker = new TactTimeChecker(_SeqName);
SequenceBuilder sb = new SequenceBuilder(1000);
SetActions(sb);
sb.OnStopped(OnStopped);
sb.OnForceError(OnForceError);
sb.Flows(
sb.Init
> sb[SEQ_ID_INIT]
> sb[SEQ_ID_SET_GRAB_FIXTURE_SEQ_PARAMS]
> sb[SEQ_ID_GRAB_FIXTURE_SEQ]
> sb[SEQ_ID_FIND_FIXTURE]
> sb[SEQ_ID_CHECK_FIXTURE_FOUND]
> sb[SEQ_ID_CHECK_SAME_LIGHT]
> sb[SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS]
> sb[SEQ_ID_GRAB_MAIN_SEQ]
> sb[SEQ_ID_FIND_MAIN_WITH_FIXTURE]
> sb[SEQ_ID_CHECK_MAIN_FOUND]
> sb[SEQ_ID_UPDATE_UI]
> sb[SEQ_ID_SAVE_IMAGE]
> sb[SEQ_ID_SEQ_COMPLETE]
> sb.Final
);
sb.Flows(
sb[SEQ_ID_CHECK_FIXTURE_FOUND] // Fixture Not Found
> sb[SEQ_ID_FIXTURE_FORCE_OK]
);
sb.Flows(
sb[SEQ_ID_CHECK_SAME_LIGHT] // Same Light : Skip Grab Main
> sb[SEQ_ID_FIND_MAIN_WITH_FIXTURE]
);
sb.Flows(
sb[SEQ_ID_CHECK_MAIN_FOUND] // Light Retry
> sb[SEQ_ID_LIGHT_RETRY]
> sb[SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS]
);
sb.Flows(
sb[SEQ_ID_LIGHT_RETRY] // Light Retry 모두 실패
> sb[SEQ_ID_MAIN_FORCE_OK]
);
sb.Flows(
sb[SEQ_ID_CHECK_MAIN_FOUND] // Main Not Found
> sb[SEQ_ID_MAIN_FORCE_OK]
);
sb.Flows(
sb[SEQ_ID_FIXTURE_FORCE_OK]
> sb[SEQ_ID_UPDATE_UI]
);
sb.Flows(
sb[SEQ_ID_MAIN_FORCE_OK]
> sb[SEQ_ID_UPDATE_UI]
);
#if SEQ_DRAW
sb.Draw($"{GetType().Name}.pdf");
#endif
return sb;
}
private SequenceActionResult Init(SequenceContext context, string id, SequenceActionResult param) {
if (param.Param != null) {
_Params = param.Param as GrabFindWithFixtureSeqParams;
context.TryGetValue<ProcParams>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_PROC_PARAMS, _Params.FovInfo.ToSubSysInfo()), out _ProcParams);
}
else {
_Logger.E($"_Params is null.");
return null;
}
_FovRecipe = VisionRecipeManager.GetFovRecipe(_Params.FovInfo);
SetLightRetry();
SetPatternAgent();
SetCalibrator();
SetCameraAgent();
SetDisplay();
return SequenceAction.Ok();
}
private void SetPatternAgent() {
var patternIndex = _Params.PatternInfo.GetPatternIndex();
_PatternAgent = PatternAgentManager.GetAgent(_Params.FovInfo);
var fixturePatternIndex = FixtureSetting.GetFixturePatternIndex(
_Params.PatternInfo.GetPatternIndex(), _Params.PatternInfo.FixtureSetting.Pattern1);
_FixturePatternInfo = _PatternAgent.GetPatternInfo(fixturePatternIndex);
}
private void SetCalibrator() {
_Calibrator = CalibratorManager.GetCalibrator(_Params.FovInfo);
}
private void SetCameraAgent() {
_CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
}
private void SetDisplay() {
_CameraDisplay = CameraDisplayManager.GetDisplay(_Params.FovInfo, _Params.DisplayType);
}
private SequenceActionResult SetGrabFixtureSeqParams(SequenceContext context, string id, SequenceActionResult param) {
return SequenceAction.Ok(
new GrabSeqParams(_Params.FovInfo, _FixturePatternInfo, _Params.DisplayType, _Params.SeqMode));
}
private SequenceActionResult FindFixture(SequenceContext context, string id, SequenceActionResult param) {
var grabImage = context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey()));
var fixturePatternResult = _PatternAgent.Find(_FixturePatternInfo, grabImage, _CameraDisplay);
_PatternAgent.Draw(_FixturePatternInfo, fixturePatternResult, _CameraDisplay, isFixturePattern: true);
GrabFindSeqUtil.SetFixturePatternResult(_Params.FovInfo, fixturePatternResult);
return SequenceAction.Ok();
}
private string CheckFixtureFound(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
var fixturePatternResult = GrabFindSeqUtil.GetFixturePatternResult(_Params.FovInfo);
if (fixturePatternResult.Status == eFoundStatus.FoundOK) {
return SEQ_ID_CHECK_SAME_LIGHT;
}
else {
return SEQ_ID_FIXTURE_FORCE_OK;
}
}
private string CheckSameLight(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
if (GrabFindSeqUtil.IsPatternAndLastLightSame(_Params.FovInfo, _Params.PatternInfo)) {
return SEQ_ID_FIND_MAIN_WITH_FIXTURE;
}
else {
return SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS;
}
}
private SequenceActionResult SetGrabMainSeqParams(SequenceContext context, string id, SequenceActionResult param) {
return SequenceAction.Ok(
new GrabSeqParams(_Params.FovInfo, _Params.PatternInfo, _Params.DisplayType, _Params.SeqMode));
}
private SequenceActionResult FindMainWithFixture(SequenceContext context, string id, SequenceActionResult param) {
var grabImage = context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey()));
var fixturePatternResult = GrabFindSeqUtil.GetFixturePatternResult(_Params.FovInfo);
var fixtureResult = fixturePatternResult.MoreInfo as FixtureResult;
_CameraDisplay.SetImage(grabImage); // Fixture Enabled && Same Light 조건에서 grabImage를 새로 Set해준다.
PatternResult patternResult = null; // Single Pattern
List<PatternResult> patternResults = null; // Multi Pattern
// Multi Pattern
if (PatternConstants.IsMultiPatternType(_Params.PatternInfo.PatternType)) {
patternResults = GrabFindSeqUtil.FindMulti(
_Params.FovInfo, _Params.PatternInfo, _PatternAgent, _Calibrator, _CameraAgent, _CameraDisplay, grabImage, fixtureResult);
patternResult = patternResults[0]; // Master Pattern 결과
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULTS, _Params.FovInfo.GetKey()), patternResults);
}
// Single Pattern
else {
patternResult = _PatternAgent.Find(_Params.PatternInfo, grabImage, _CameraDisplay, fixtureResult);
GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref patternResult);
_PatternAgent.Draw(_Params.PatternInfo, patternResult, _CameraDisplay);
//_CameraDisplay.DrawRefCross();
}
patternResult.PatternName = _Params.PatternInfo.PatternName;
patternResult.PatternType = _Params.PatternInfo.PatternType;
patternResult.FovId = _Params.FovInfo.FovId;
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()), patternResult);
return SequenceAction.Ok();
}
private string CheckMainFound(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
if (mainPatternResult.Status == eFoundStatus.FoundOK) {
return SEQ_ID_UPDATE_UI;
}
else {
if (_ProcParams != null && _ProcParams.LightRetryParams.LightRetryEnabled)
{
return SEQ_ID_LIGHT_RETRY;
}
else {
return SEQ_ID_MAIN_FORCE_OK;
}
}
}
private string LightRetry(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
if (_OriginalLightInfo == null) {
_OriginalLightInfo = _Params.PatternInfo.LightInfo.Clone();
}
_CurLightRetryCnt++;
if (_CurLightRetryCnt <= _ProcParams.LightRetryParams.LightRetryCount)
{
var lightParams = _Params.PatternInfo.LightInfo.LightParams;
var lightLevelMsg = string.Empty;
for (int i = 0; i < lightParams.Count; i++)
{
var lightParam = lightParams[i];
lightParam.Level = IsLightRetryStepMode(_ProcParams.LightRetryParams.LightRetryMode) ?
Convert.ToInt16(GetLightRetryValueStepMode(_CurLightRetryCnt, lightParam.Level, _ProcParams.LightRetryParams)) :
Convert.ToInt16(GetLightRetryValueRatioMode(_CurLightRetryCnt, lightParam.Level, _OriginalLightInfo.LightParams[i].Level, _ProcParams.LightRetryParams));
lightLevelMsg += $"[{i} | {lightParam.Level}] ";
}
PrintLogAndShowToast(_Logger,
$"[{_Params.FovInfo.SubSysName}][{_Params.FovInfo.FovName}] " +
$"Grab retried at light step {_ProcParams.LightRetryParams.LightRetryStep}." +
$"Mode is {_ProcParams.LightRetryParams.LightRetryMode.ToString()}. " +
$"{lightLevelMsg} [{_CurLightRetryCnt}/{_ProcParams.LightRetryParams.LightRetryCount}]");
return SEQ_ID_SET_GRAB_MAIN_SEQ_PARAMS; // 조명 Retry 시도
}
_Params.PatternInfo.LightInfo = _OriginalLightInfo.Clone(); // 기존 조명 원복
return SEQ_ID_MAIN_FORCE_OK; // 조명 Retry 모두 실패시, 강제 OK 시도
}
private SequenceActionResult ForceOK(SequenceContext context, string id, SequenceActionResult param) {
if (_ProcParams != null && _ProcParams.ForceOkEnabled &&
PatternConstants.IsForceOkSupport(_Params.PatternInfo.PatternType)) {
bool isMainPattern = true;
PatternResult patternResult;
if (id == SEQ_ID_FIXTURE_FORCE_OK) {
isMainPattern = false;
}
if (isMainPattern) {
patternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
}
else {
patternResult = GrabFindSeqUtil.GetFixturePatternResult(_Params.FovInfo);
}
patternResult = GrabFindSeqUtil.DoForceOk(new ForceOkReqParam(
_Params.FovInfo,
patternResult.PatternIndex,
context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey())),
patternResult));
if (patternResult.Status == eFoundStatus.FoundOK || patternResult.Status == eFoundStatus.ForceOK) {
_CameraDisplay.ClearMeasureGraphics();
GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref patternResult);
DrawForceOkResult(patternResult);
if (isMainPattern) {
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()), patternResult);
}
else {
GrabFindSeqUtil.SetFixturePatternResult(_Params.FovInfo, patternResult);
}
}
}
return SequenceAction.Ok();
}
private void DrawForceOkResult(PatternResult patternResult) {
_PatternAgent.Draw(_Params.PatternInfo, patternResult, _CameraDisplay, isFixturePattern: false);
var result = patternResult.Results.ToList()[0];
eColor color = eColor.DarkRed;
_CameraDisplay.AddCross(new Cross(result.ImagePosition, 80), color);
}
private SequenceActionResult UpdateUI(SequenceContext context, string id, SequenceActionResult param) {
var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
VisionEnv.AuroraEnv.EventPublisher.Publish(new EventMessage(
_Params.FovInfo.GetKey(), VisionEventId.UPDATE_PATTERN_RESULT, mainPatternResult));
return SequenceAction.Ok();
}
private SequenceActionResult SaveImage(SequenceContext context, string id, SequenceActionResult param) {
var grabImage = context.GetValue<object>(string.Format(VisionContextKeys.CNTXT_KEY_GRAB_IMAGE, _Params.FovInfo.GetKey()));
var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
var materialInfo = context.GetValue<MaterialInfo>(VisionContextKeys.CNTXT_KEY_MATERIAL_INFO);
if (mainPatternResult.Status == eFoundStatus.FoundOK) {
if (VisionSettingManager.VisionSetting.SaveOkImage == false) {
return SequenceAction.Ok();
}
}
_ImageManager.SaveImage(_Params.FovInfo, grabImage, mainPatternResult.Status.ToString(), materialInfo);
return SequenceAction.Ok();
}
private SequenceActionResult SeqComplete(SequenceContext context, string id, SequenceActionResult param) {
var mainPatternResult = context.GetValue<PatternResult>(string.Format(VisionContextKeys.CNTXT_KEY_PATTERN_RESULT, _Params.FovInfo.GetKey()));
_TactTimeChecker.Stop(TactTimeType.ProcSeq, "Light/Grab/Find");
return SequenceAction.Ok(mainPatternResult);
}
void OnStopped(
SequenceContext context) {
_Logger.I($"{context.SequenceId} - STOP Handler");
HandleAbnormalFinish(context);
}
void OnForceError(
SequenceContext context, string id) {
_Logger.E($"{context.SequenceId} - FORCE ERROR Handler");
HandleAbnormalFinish(context);
}
void HandleAbnormalFinish(
SequenceContext context) {
_TactTimeChecker.Stop(TactTimeType.ProcSeq, "Light/Grab/Find");
if(_Params!=null && _OriginalLightInfo !=null) _Params.PatternInfo.LightInfo = _OriginalLightInfo.Clone(); // 기존 조명 원복 ( 방어코드 )
}
}
}