2026-01-26 14:51:28 +09:00

544 lines
22 KiB
C#

/*
* Copyright (c) 2025 LG Electronics Inc.
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
*/
using Aurora.Api;
using VisionCommon;
using VisionAligner;
using VisionCalibrator;
using VisionSequence.Pattern;
using VisionSequence.Camera;
using System.Collections.Generic;
using System.Linq;
namespace VisionSequence.Aligner {
/// <summary>
/// C1_F1_AP2_A2 얼라인 시퀀스 파라미터
/// </summary>
public class AlignSeqParams_C1_F1_AP2_A2 : SeqParamsBase {
/// <summary>얼라인 키 정보</summary>
public AlignInfo AlignInfo { get; set; }
/// <summary>얼라인 레퍼런스 위치</summary>
public Dictionary<int, StaticPoint> ReferenceResults { get; set; }
/// <summary>얼라인 레퍼런스 정보</summary>
public RefDistParam RefDistParam { get; set; }
/// <summary>플라잉 위치 인덱스 (Optional)</summary>
public object GrabImage { get; set; }
/// <summary>메인 화면 타입</summary>
public eDisplayType DisplayType { get; set; }
/// <summary>
/// C1_F1_AP2_A2 얼라인 시퀀스 파라미터를 얻어온다.
/// </summary>
/// <param name="alignInfo">얼라인 키 정보</param>
/// <param name="referenceResults">얼라인 레퍼런스 위치</param>
/// <param name="grabImage">그랩 이미지 (Optional)</param>
/// <param name="refDistParam">센터로부터 각 패턴의 정위치까지 떨어진 거리 파라미터</param>
/// <param name="displayType">메인 화면 타입</param>
/// <param name="seqMode">시퀀스 모드</param>
public AlignSeqParams_C1_F1_AP2_A2(
AlignInfo alignInfo,
Dictionary<int, StaticPoint> referenceResults,
RefDistParam refDistParam,
object grabImage = null,
eDisplayType displayType = eDisplayType.Main_1,
eSeqMode seqMode = eSeqMode.Auto) : base(seqMode) {
this.AlignInfo = alignInfo;
this.ReferenceResults = referenceResults;
this.RefDistParam = refDistParam;
this.GrabImage = grabImage;
this.DisplayType = displayType;
}
}
/// <summary>
/// CAM 1대 / FOV 1개 / 서로 다른(Another) 패턴 2개 / 얼라인 2개 / 1 Point Align 2개 / MeasurePattern to (Center + Target Offset)
/// </summary>
public class AlignSeq_C1_F1_AP2_A2 : SubSeqBase {
private readonly Logger LOG = new Logger(typeof(AlignSeq_C1_F1_AP2_A2).Name);
private AlignSeqParams_C1_F1_AP2_A2 _Params;
private FovInfo _FovInfo;
private AlignRecipe _AlignRecipe;
private SubSysInfo _SubSysInfo;
private PatternAgentBase _PatternAgent;
private VisionAligner.Aligner _Aligner;
private Calibrator _Calibrator;
private CameraAgentBase _CameraAgent;
private ICameraDisplay _CameraDisplay;
private CsvManager _CsvManager = new CsvManager();
private ImageManager _ImageManager = new ImageManager();
private Dictionary<int, PatternResult> _MeasurePatternResults = new Dictionary<int, PatternResult>();
private Dictionary<int, BaseResult> _MeasureResults = new Dictionary<int, BaseResult>();
private Dictionary<int, AlignResult> _AlignResults = new Dictionary<int, AlignResult>();
private string _SeqName;
#region Sequence Id
private const string SEQ_ID_INIT = "INIT";
private const string SEQ_ID_CLEAR = "CLEAR";
private const string SEQ_ID_SET_FIXTURE = "SET_FIXTURE";
private const string SEQ_ID_FORK = "FORK";
private const string SEQ_ID_FIND_1 = "FIND_1";
private const string SEQ_ID_CHECK_FOUND_1 = "CHECK_FOUND_1";
private const string SEQ_ID_SET_MEASURE_1 = "SET_MEASURE_1";
private const string SEQ_ID_ALIGN_1 = "ALIGN_1";
private const string SEQ_ID_ALIGN_EXIT_1 = "ALIGN_EXIT_1";
private const string SEQ_ID_FIND_2 = "FIND_2";
private const string SEQ_ID_CHECK_FOUND_2 = "CHECK_FOUND_2";
private const string SEQ_ID_SET_MEASURE_2 = "SET_MEASURE_2";
private const string SEQ_ID_ALIGN_2 = "ALIGN_2";
private const string SEQ_ID_ALIGN_EXIT_2 = "ALIGN_EXIT_2";
private const string SEQ_ID_JOIN = "JOIN";
private const string SEQ_ID_UPDATE_UI = "UPDATE_UI";
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
#endregion
/// <summary>
/// 생성자
/// </summary>
public AlignSeq_C1_F1_AP2_A2() {
}
private void SetActions(SequenceBuilder sb) {
var grabSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabSeq(), typeof(GrabSeq).Name);
sb.AddAction(SEQ_ID_INIT, Init);
sb.AddAction(SEQ_ID_CLEAR, Clear);
sb.AddAction(SEQ_ID_SET_FIXTURE, SetFixture);
sb.AddFork(SEQ_ID_FORK);
sb.AddAction(SEQ_ID_FIND_1, Find);
sb.AddDecision(SEQ_ID_CHECK_FOUND_1, CheckFound);
sb.AddAction(SEQ_ID_SET_MEASURE_1, SetMeasure);
sb.AddAction(SEQ_ID_ALIGN_1, Align);
sb.AddAction(SEQ_ID_ALIGN_EXIT_1, AlignExit);
sb.AddAction(SEQ_ID_FIND_2, Find);
sb.AddDecision(SEQ_ID_CHECK_FOUND_2, CheckFound);
sb.AddAction(SEQ_ID_SET_MEASURE_2, SetMeasure);
sb.AddAction(SEQ_ID_ALIGN_2, Align);
sb.AddAction(SEQ_ID_ALIGN_EXIT_2, AlignExit);
sb.AddJoin(SEQ_ID_JOIN);
sb.AddAction(SEQ_ID_UPDATE_UI, UpdateUI);
sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
}
public override SequenceBuilder Builder(string seqName) {
_SeqName = 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_CLEAR]
> sb[SEQ_ID_SET_FIXTURE]
> sb[SEQ_ID_FORK]
> sb[SEQ_ID_FIND_1]
> sb[SEQ_ID_CHECK_FOUND_1]
> sb[SEQ_ID_SET_MEASURE_1]
> sb[SEQ_ID_ALIGN_1]
> sb[SEQ_ID_ALIGN_EXIT_1]
> sb[SEQ_ID_JOIN]
> sb[SEQ_ID_UPDATE_UI]
> sb[SEQ_ID_SEQ_COMPLETE]
);
sb.Flows(
sb[SEQ_ID_FORK]
> sb[SEQ_ID_FIND_2]
> sb[SEQ_ID_CHECK_FOUND_2]
> sb[SEQ_ID_SET_MEASURE_2]
> sb[SEQ_ID_ALIGN_2]
> sb[SEQ_ID_ALIGN_EXIT_2]
> sb[SEQ_ID_JOIN]
);
sb.Flows(
sb[SEQ_ID_CHECK_FOUND_1] // Measure 1 NG
> sb[SEQ_ID_ALIGN_EXIT_1]
);
sb.Flows(
sb[SEQ_ID_CHECK_FOUND_2] // Measure 2 NG
> sb[SEQ_ID_ALIGN_EXIT_2]
);
sb.Flows(
sb[SEQ_ID_SEQ_COMPLETE]
> sb.Final
);
#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 AlignSeqParams_C1_F1_AP2_A2;
}
else {
LOG.E($"Param is null.");
return null;
}
_AlignRecipe = VisionRecipeManager.GetAlignRecipe(_Params.AlignInfo);
_SubSysInfo = VisionRecipeManager.GetSubSysInfo(_Params.AlignInfo.UnitId, _Params.AlignInfo.SubSysId);
_FovInfo = VisionRecipeManager.GetFovInfo(_SubSysInfo, eFovId.Fov1);
_PatternAgent = PatternAgentManager.GetAgent(_FovInfo);
_Aligner = AlignAgentManager.GetAlignAgent(_Params.AlignInfo);
_Calibrator = CalibratorManager.GetCalibrator(_FovInfo);
_CameraAgent = CameraAgentManager.GetCameraAgent(_FovInfo);
_CameraDisplay = CameraDisplayManager.GetDisplay(_FovInfo, _Params.DisplayType);
_VisionLog = new Logger(_SubSysInfo.SubSysName, VisionKeys.LogKey);
InitProcData();
return SequenceAction.Ok();
}
private void InitProcData() {
for (int patternPosIdx = 0; patternPosIdx < _Params.ReferenceResults.Count; patternPosIdx++) {
if (!_AlignResults.ContainsKey(patternPosIdx)) {
_AlignResults.Add(patternPosIdx, null);
}
else {
_AlignResults[patternPosIdx] = null;
}
if (!_MeasurePatternResults.ContainsKey(patternPosIdx)) {
_MeasurePatternResults.Add(patternPosIdx, null);
}
else {
_MeasurePatternResults[patternPosIdx] = null;
}
if (!_MeasureResults.ContainsKey(patternPosIdx)) {
_MeasureResults.Add(patternPosIdx, null);
}
else {
_MeasureResults[patternPosIdx] = null;
}
}
}
private SequenceActionResult Clear(
SequenceContext context,
string id,
SequenceActionResult param) {
return SequenceAction.Ok();
}
private SequenceActionResult SetFixture(
SequenceContext context,
string id,
SequenceActionResult param) {
var procPatternInfo_1 = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
var procPatternInfo_2 = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern2);
var patternInfo_1 = GetPatternInfo(_Params.AlignInfo, _FovInfo, procPatternInfo_1);
var patternInfo_2 = GetPatternInfo(_Params.AlignInfo, _FovInfo, procPatternInfo_2);
PatternIndex fixturePatternIndex = null;
if (patternInfo_1.FixtureSetting.Enabled) {
fixturePatternIndex = FixtureSetting.GetFixturePatternIndex(patternInfo_1.GetPatternIndex(), patternInfo_1.FixtureSetting.Pattern1);
}
else if (patternInfo_2.FixtureSetting.Enabled) {
fixturePatternIndex = FixtureSetting.GetFixturePatternIndex(patternInfo_2.GetPatternIndex(), patternInfo_2.FixtureSetting.Pattern1);
}
PatternResult fixturePatternResult = null;
if (fixturePatternIndex != null) {
var fixturePatternInfo = _PatternAgent.GetPatternInfo(fixturePatternIndex);
fixturePatternResult = _PatternAgent.Find(fixturePatternInfo, _Params.GrabImage, _CameraDisplay);
GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref fixturePatternResult);
_PatternAgent.Draw(fixturePatternInfo, fixturePatternResult, _CameraDisplay, isFixturePattern: true);
}
return SequenceAction.Ok(fixturePatternResult);
}
private SequenceActionResult Find(
SequenceContext context,
string id,
SequenceActionResult param) {
ProcPatternInfo procPatternInfo = null;
switch (id) {
case SEQ_ID_FIND_1:
procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
break;
case SEQ_ID_FIND_2:
procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern2);
break;
}
var patternInfo = GetPatternInfo(_Params.AlignInfo, _FovInfo, procPatternInfo);
PatternResult patternResult = null;
if (patternInfo.FixtureSetting.Enabled) {
var fixturePatternResult = param.Param as PatternResult;
var fixtureResult = fixturePatternResult.MoreInfo as FixtureResult;
patternResult = _PatternAgent.Find(patternInfo, _Params.GrabImage, _CameraDisplay, fixtureResult);
GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref patternResult);
_PatternAgent.Draw(patternInfo, patternResult, _CameraDisplay);
}
else {
patternResult = _PatternAgent.Find(patternInfo, _Params.GrabImage, _CameraDisplay);
GrabFindSeqUtil.TransformCoord(_Calibrator, _CameraAgent.GetImageSize(), ref patternResult);
_PatternAgent.Draw(patternInfo, patternResult, _CameraDisplay, isFixturePattern: false);
}
patternResult.PatternName = patternInfo.PatternName;
patternResult.PatternType = patternInfo.PatternType;
patternResult.FovId = _FovInfo.FovId;
return SequenceAction.Ok(patternResult);
}
private string CheckFound(
SequenceContext context,
string id,
ICollection<string> branches,
SequenceActionResult param) {
var patternResult = param.Param as PatternResult;
SetMeasurePatterResults(id, patternResult);
if (patternResult.Status == eFoundStatus.FoundOK || patternResult.Status == eFoundStatus.ForceOK) {
if (id == SEQ_ID_CHECK_FOUND_1) {
return SEQ_ID_SET_MEASURE_1;
}
else {
return SEQ_ID_SET_MEASURE_2;
}
}
else {
// Mark NG
var alignResult = new AlignResult() {
Status = AlignStatus.NOT_FOUND,
Delta = new AlignDelta(),
AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
};
if (id == SEQ_ID_CHECK_FOUND_1) {
int patternPosIdx = 0;
alignResult.AlignName = $"Align{patternPosIdx}";
_AlignResults[patternPosIdx] = alignResult;
return SEQ_ID_ALIGN_EXIT_1;
}
else {
int patternPosIdx = _Params.ReferenceResults.Count - 1;
alignResult.AlignName = $"Align{patternPosIdx}";
_AlignResults[patternPosIdx] = alignResult;
return SEQ_ID_ALIGN_EXIT_2;
}
}
}
private void SetMeasurePatterResults(string id, PatternResult patternResult) {
if (id == SEQ_ID_CHECK_FOUND_1) {
int patternPosIdx = 0;
_MeasurePatternResults[patternPosIdx] = patternResult;
}
else {
int patternPosIdx = _Params.ReferenceResults.Count - 1;
_MeasurePatternResults[patternPosIdx] = patternResult;
}
}
private SequenceActionResult SetMeasure(
SequenceContext context,
string id,
SequenceActionResult param) { // Param : PatternResult
var patternResult = param.Param as PatternResult;
var baseResults = patternResult.Results.ToList();
int patternPosIdx = 0;
var refDists = _Params.RefDistParam.RefDists;
switch (id) {
case SEQ_ID_SET_MEASURE_1:
// 이미지 좌표 기준 좌상단
patternPosIdx = 0;
_MeasureResults[patternPosIdx] = baseResults[0];
break;
case SEQ_ID_SET_MEASURE_2:
// 이미지 좌표 기준 우하단
if (refDists == null || refDists.Count < _Params.ReferenceResults.Count) {
patternPosIdx = _Params.ReferenceResults.Count - 1;
}
else {
patternPosIdx = refDists.Count - 1;
}
_MeasureResults[patternPosIdx] = baseResults[0];
break;
}
return SequenceAction.Ok();
}
private SequenceActionResult Align(
SequenceContext context,
string id,
SequenceActionResult param) {
int patternPosIdx = 0;
var refDists = _Params.RefDistParam.RefDists;
switch (id) {
case SEQ_ID_ALIGN_1:
// 이미지 좌표 기준 좌상단
patternPosIdx = 0;
break;
case SEQ_ID_ALIGN_2:
// 이미지 좌표 기준 우하단
if (refDists == null || refDists.Count < _Params.ReferenceResults.Count) {
patternPosIdx = _Params.ReferenceResults.Count - 1;
}
else {
patternPosIdx = refDists.Count - 1;
}
break;
}
var baseResult = _MeasureResults[patternPosIdx];
var meaPoint = baseResult.MaterialPosition;
var refPoint = _Params.ReferenceResults[patternPosIdx];
var meaAngle = baseResult.Angle;
var flipY = (int)_Calibrator.GetFlipY();
var alignResult = _Aligner.Align(meaPoint, refPoint, meaAngle, flipY);
if (alignResult == null) {
LOG.FuncE("alignResult is null.");
return SequenceAction.Error();
}
alignResult.AlignCnt = patternPosIdx;
alignResult.DisplayType = _Params.DisplayType;
alignResult.AlignName = $"Align{(patternPosIdx)}";
alignResult.AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm;
_AlignResults[patternPosIdx] = alignResult;
return SequenceAction.Ok();
}
private SequenceActionResult AlignExit(
SequenceContext context,
string id,
SequenceActionResult param) {
// 얼라인 정상 종료, 혹은 패턴 NotFound시 지나가는 노드
return SequenceAction.Ok();
}
private SequenceActionResult UpdateUI(
SequenceContext context,
string id,
SequenceActionResult param) {
SortProcData();
UpdateProcData();
PrintVisionLog();
return SequenceAction.Ok();
}
private void SortProcData() {
// 패턴 위치 인덱스를 기준으로 패턴 및 얼라인 결과 오름차순 정렬
_MeasurePatternResults = _MeasurePatternResults.OrderBy(item => item.Key).ToDictionary(x => x.Key, x => x.Value);
_MeasureResults = _MeasureResults.OrderBy(item => item.Key).ToDictionary(x => x.Key, x => x.Value);
_AlignResults = _AlignResults.OrderBy(item => item.Key).ToDictionary(x => x.Key, x => x.Value);
}
private void UpdateProcData() {
List<PatternResult> cloneMeasurePatternResults = _MeasurePatternResults.Values.Select(v => v?.Clone()).ToList();
// 얼라인 탭에 패턴 및 얼라인 로그 기록
VisionEnv.AuroraEnv.EventPublisher.Publish(
new EventMessage(_FovInfo.GetKey(), VisionEventId.UPDATE_PATTERN_RESULTS, cloneMeasurePatternResults));
Dictionary<int, AlignResult> cloneAlignResults = _AlignResults.ToDictionary(x => x.Key, x => x.Value?.Clone());
Dictionary<int, BaseResult> cloneMeasureResults = _MeasureResults.ToDictionary(x => x.Key, x => x.Value?.Clone());
var subSysInfo = _Params.AlignInfo.ToSubSysInfo();
var alignResultsParam = new AlignResultsParam(cloneMeasureResults, cloneAlignResults);
VisionEnv.AuroraEnv.EventPublisher.Publish(
new EventMessage(_Params.AlignInfo.GetKey(), VisionEventId.UPDATE_ALIGN_RESULTS, alignResultsParam));
}
private void PrintVisionLog() {
foreach (var itemPairs in _AlignResults) {
var alignResult = itemPairs.Value;
PrintVisionLog(alignResult);
}
}
private SequenceActionResult SeqComplete(
SequenceContext context,
string id,
SequenceActionResult param) {
SetPatternResults();
return SequenceAction.Ok(_AlignResults);
}
private void SetPatternResults() {
foreach (var itemPair in _AlignResults) {
int patternPosIdx = itemPair.Key;
AlignResult alignResult = itemPair.Value;
if (_MeasurePatternResults.ContainsKey(patternPosIdx)) {
if (!alignResult.PatternResults.ContainsKey(_FovInfo)) {
alignResult.PatternResults.Add(_FovInfo, _MeasurePatternResults[patternPosIdx]);
}
else {
alignResult.PatternResults[_FovInfo] = _MeasurePatternResults[patternPosIdx];
}
}
}
}
void OnStopped(
SequenceContext context) {
LOG.I($"{context.SequenceId} - STOP Handler");
HandleAbnormalFinish(context);
}
void OnForceError(
SequenceContext context, string id) {
LOG.E($"{context.SequenceId} - FORCE ERROR Handler");
HandleAbnormalFinish(context);
}
void HandleAbnormalFinish(
SequenceContext context) {
}
}
}