2026-06-05 13:44:39 +09:00

534 lines
20 KiB
C#

using Aurora.Api;
using Aurora.Api.Devices;
using VisionCommon;
using VisionAligner;
using VisionCalibrator;
using VisionSequence.Pattern;
using VisionSequence.Inspector;
using Hermes;
using System.Collections.Generic;
using System.Linq;
using VisionInspector;
using VisionCommonUI;
using VisionCommonUI.Resources;
namespace VisionSequence.Aligner {
/// <summary>
/// C1_FN_MV 얼라인 시퀀스 파라미터
/// </summary>
public class AlignSeqParams_C1_FN_MV : SeqParamsBase {
/// <summary>얼라인 키 정보</summary>
public AlignInfo AlignInfo { get; set; }
/// <summary>FOV별로 이동시킬 절대위치</summary>
public Dictionary<eFovId, Position> FovPositions { get; set; }
/// <summary>메인 화면 타입</summary>
public eDisplayType DisplayType { get; set; }
/// <summary>
/// C1_FN_MV 얼라인 시퀀스 파라미터를 얻어온다.
/// </summary>
/// <param name="alignInfo">얼라인 키 정보</param>
/// <param name="fovPositions">FOV별로 이동시킬 절대위치. null일 경우, 자동으로 Adapter에 정의된 티칭위치로 이동</param>
/// <param name="displayType">메인 화면 타입</param>
/// <param name="seqMode">시퀀스 모드</param>
public AlignSeqParams_C1_FN_MV(
AlignInfo alignInfo,
Dictionary<eFovId, Position> fovPositions = null,
eDisplayType displayType = eDisplayType.Main_1,
eSeqMode seqMode = eSeqMode.Auto) : base(seqMode) {
this.AlignInfo = alignInfo;
this.FovPositions = fovPositions;
this.DisplayType = displayType;
}
}
/// <summary>
/// CAM 1대 / FOV N개 (2 <= N <= 4) / 포지션 이동 (O) / 2 Point Align / 2 MeasurePatterns to 2 ReferencePatterns
/// </summary>
public class AlignSeq_C1_FN_MV : SubSeqBase {
private const int MEASURE_MAX_CNT = 2; // 얼라인을 수행하기 위해 찾아야 하는 패턴의 수
private readonly Logger LOG = new Logger(typeof(AlignSeq_C1_FN_MV).Name);
private AlignSeqParams_C1_FN_MV _Params;
private AlignRecipe _AlignRecipe;
private SubSysInfo _SubSysInfo;
private VisionAligner.Aligner _Aligner;
private Calibrator _Calibrator;
private PatternAgentBase _PatternAgent;
private Dictionary<eFovId, FovInfo> _FovInfos = new Dictionary<eFovId, FovInfo>();
private Dictionary<eFovId, BaseResult> _MeasureResults = new Dictionary<eFovId, BaseResult>();
private Dictionary<eFovId, BaseResult> _ReferenceResults = new Dictionary<eFovId, BaseResult>();
private eFovId _CurFovId = eFovId.Fov1;
private string _SeqName;
private int _FovNum = 0;
private bool _IsForceOk = false;
#region Sequence Id
private const string SEQ_ID_INIT = "INIT";
private const string SEQ_ID_CLEAR = "CLEAR";
private const string SEQ_ID_FORK = "FORK";
private const string SEQ_ID_MOVE_FOV_POS = "MOVE_FOV_POS";
private const string SEQ_ID_SET_FIND_MEASURE_SEQ_PARAMS = "SET_FIND_MEASURE_SEQ_PARAMS";
private const string SEQ_ID_FIND_MEASURE_SEQ = "FIND_MEASURE_SEQ";
private const string SEQ_ID_CHECK_MEASURE_FOUND = "CHECK_MEASURE_FOUND";
private const string SEQ_ID_MARK_NG = "MARK_NG";
private const string SEQ_ID_SET_MEASURE = "SET_MEASURE";
private const string SEQ_ID_CHECK_MEASURE_FINISHED = "CHECK_MEASURE_FINISHED";
private const string SEQ_ID_SET_REFERENCE = "SET_REFERENCE";
private const string SEQ_ID_JOIN = "JOIN";
private const string SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED = "CHECK_PATTERN_DISTANCE_INSP_ENABLED";
private const string SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS = "SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS";
private const string SEQ_ID_PATTERN_DISTANCE_INSP_SEQ = "PATTERN_DISTANCE_INSP_SEQ";
private const string SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS = "CHECK_PATTERN_DISTANCE_INSP";
private const string SEQ_ID_DISTANCE_NG = "DISTANCE_NG";
private const string SEQ_ID_ALIGN = "ALIGN";
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
#endregion
/// <summary>
/// 생성자
/// </summary>
public AlignSeq_C1_FN_MV() {
}
private void SetActions(SequenceBuilder sb) {
SequenceBuilder grabFindProcSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindProcSeq(), typeof(GrabFindProcSeq).Name);
SequenceBuilder patternDistanceInspSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new InspSeq_PatternDistance(), typeof(InspSeq_PatternDistance).Name);
sb.AddAction(SEQ_ID_INIT, Init);
sb.AddAction(SEQ_ID_CLEAR, Clear);
sb.AddFork(SEQ_ID_FORK);
sb.AddAction(SEQ_ID_MOVE_FOV_POS, MoveFovPos);
sb.AddAction(SEQ_ID_SET_FIND_MEASURE_SEQ_PARAMS, SetFindMeasureSeqParams);
sb.AddSubsequence(SEQ_ID_FIND_MEASURE_SEQ, grabFindProcSeqBuilder);
sb.AddDecision(SEQ_ID_CHECK_MEASURE_FOUND, CheckFound);
sb.AddAction(SEQ_ID_MARK_NG, MarkNG);
sb.AddAction(SEQ_ID_SET_MEASURE, SetMeasure);
sb.AddDecision(SEQ_ID_CHECK_MEASURE_FINISHED, CheckMeasureFinished);
sb.AddAction(SEQ_ID_SET_REFERENCE, SetReference);
sb.AddJoin(SEQ_ID_JOIN);
// 패턴 거리 검사
sb.AddDecision(SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED, CheckPatternDistanceInspEnabled);
sb.AddAction(SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS, SetPatternDistanceInspSeqParams);
sb.AddSubsequence(SEQ_ID_PATTERN_DISTANCE_INSP_SEQ, patternDistanceInspSeqBuilder);
sb.AddDecision(SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS, CheckPatternDistanceInspStatus);
sb.AddAction(SEQ_ID_DISTANCE_NG, DistanceNG);
sb.AddAction(SEQ_ID_ALIGN, Align);
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_FORK]
> sb[SEQ_ID_SET_FIND_MEASURE_SEQ_PARAMS]
> sb[SEQ_ID_FIND_MEASURE_SEQ]
> sb[SEQ_ID_CHECK_MEASURE_FOUND]
> sb[SEQ_ID_SET_MEASURE]
> sb[SEQ_ID_CHECK_MEASURE_FINISHED]
> sb[SEQ_ID_JOIN]
> sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED] // 패턴 거리 검사
> sb[SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS]
> sb[SEQ_ID_PATTERN_DISTANCE_INSP_SEQ]
> sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS]
> sb[SEQ_ID_ALIGN]
> sb[SEQ_ID_SEQ_COMPLETE]
);
sb.Flows(
sb[SEQ_ID_FORK]
> sb[SEQ_ID_SET_REFERENCE]
> sb[SEQ_ID_JOIN]
);
sb.Flows(
sb[SEQ_ID_CHECK_MEASURE_FINISHED]
> sb[SEQ_ID_MOVE_FOV_POS]
);
sb.Flows(
sb[SEQ_ID_CHECK_MEASURE_FINISHED] // Measure Mark NG -> Finish Seq
> sb[SEQ_ID_MARK_NG]
);
sb.Flows(
sb[SEQ_ID_CHECK_MEASURE_FOUND] // Measure Mark NG -> Check Measure Finish
> sb[SEQ_ID_CHECK_MEASURE_FINISHED]
);
sb.Flows(
sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_ENABLED] // Pattern Distance OK
> sb[SEQ_ID_ALIGN]
);
sb.Flows(
sb[SEQ_ID_CHECK_PATTERN_DISTANCE_INSP_STATUS] // Pattern Distance NG
> sb[SEQ_ID_DISTANCE_NG]
);
sb.Flows(
sb[SEQ_ID_MARK_NG]
> sb[SEQ_ID_SEQ_COMPLETE]
);
sb.Flows(
sb[SEQ_ID_DISTANCE_NG]
> sb[SEQ_ID_SEQ_COMPLETE]
);
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_FN_MV;
}
else {
LOG.E($"Param is null.");
return null;
}
_IsForceOk = false;
_AlignRecipe = VisionRecipeManager.GetAlignRecipe(_Params.AlignInfo);
_SubSysInfo = _Params.AlignInfo.ToSubSysInfo();
_PatternAgent = FovPatternAgentManager.GetAgent(_SubSysInfo);
_Aligner = AlignAgentManager.GetAlignAgent(_Params.AlignInfo);
SetFovInfos();
_Calibrator = CalibratorManager.GetCalibrator(_FovInfos[eFovId.Fov1]);
return SequenceAction.Ok();
}
private void SetFovInfos() {
foreach (FovInfo fovInfo in VisionRecipeManager.GetFovInfos(_SubSysInfo)) {
if (!_FovInfos.ContainsKey(fovInfo.FovId)) {
_FovInfos.Add(fovInfo.FovId, fovInfo);
}
}
_FovNum = ProcParams.GetFovNum(_AlignRecipe.ProcParams.ProcFovType);
}
private SequenceActionResult Clear(
SequenceContext context,
string id,
SequenceActionResult param) {
_CurFovId = eFovId.Fov1;
_MeasureResults.Clear();
_ReferenceResults.Clear();
return SequenceAction.Ok();
}
private SequenceActionResult SetFindMeasureSeqParams(
SequenceContext context,
string id,
SequenceActionResult param) {
var procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfos[_CurFovId], _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
var fovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfos[_CurFovId]);
PatternIndex patternIndex = null;
if (procPatternInfo.IsFovPattern) {
_PatternAgent = FovPatternAgentManager.GetAgent(_SubSysInfo);
patternIndex = new FovPatternIndex(_FovInfos[_CurFovId], fovRecipe.FovName, procPatternInfo.PatternId);
}
else {
_PatternAgent = CamPatternAgentManager.GetAgent(fovRecipe.CamRecipe.CamId);
patternIndex = new CamPatternIndex(fovRecipe.CamRecipe.CamId, procPatternInfo.PatternId);
}
PatternInfo patternInfo = _PatternAgent.GetPatternInfo(patternIndex);
patternInfo.PatternName = procPatternInfo.PatternName;
return SequenceAction.Ok(new GrabFindProcSeqParams(_FovInfos[_CurFovId], patternInfo, _Params.DisplayType, _Params.SeqMode));
}
private SequenceActionResult SetMeasure(
SequenceContext context,
string id,
SequenceActionResult param) { // Param : PatternResult
var patternResult = param.Param as PatternResult;
var baseResults = patternResult.Results.ToList();
_MeasureResults.Add(_CurFovId, baseResults[0]);
return SequenceAction.Ok();
}
private string CheckMeasureFinished(
SequenceContext context,
string id,
ICollection<string> branches,
SequenceActionResult param) {
if (_MeasureResults.Count == MEASURE_MAX_CNT) {
return SEQ_ID_JOIN;
}
if (HasNextFov()) {
if (IsPossibleToFindAll()) {
IncreaseCurFovId();
return SEQ_ID_MOVE_FOV_POS;
}
}
return SEQ_ID_MARK_NG;
}
private bool HasNextFov() {
return _CurFovId < _FovInfos.Keys.Max();
}
private bool IsPossibleToFindAll() {
int lastFovIdx = (int)_FovInfos.Keys.Max();
int curFovIdx = (int)_CurFovId;
int possibleFoundCount = lastFovIdx - curFovIdx;
int curFoundCount = _MeasureResults.Count;
return MEASURE_MAX_CNT <= (possibleFoundCount + curFoundCount);
}
private SequenceActionResult SetReference(
SequenceContext context,
string id,
SequenceActionResult param) {
foreach (var fovInfoPair in _FovInfos) {
eFovId fovId = fovInfoPair.Key;
var refMaterialPos = VisionRecipeManager.GetRefPoint(_SubSysInfo, fovId);
_ReferenceResults.Add(
fovId,
new BaseResult() { MaterialPosition = refMaterialPos.Clone() as StaticPoint });
}
return SequenceAction.Ok();
}
private SequenceActionResult MoveFovPos(
SequenceContext context,
string id,
SequenceActionResult param) {
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
return SequenceAction.Ok();
}
if(_CurFovId == eFovId.Fov1) {
return SequenceAction.Ok();
}
int res = E1.GENERIC;
if (_Params.FovPositions != null && _Params.FovPositions.ContainsKey(_CurFovId)) {
// 파라미터로 넘겨받은 절대위치로 이동
res = EquipAdapter.Instance.TeachingMotionManager.AbsMove(_FovInfos[_CurFovId], _Params.FovPositions[_CurFovId]);
}
else {
// Adapter에 정의된 티칭위치로 이동
res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_FovInfos[_CurFovId], ePositionType.Teaching);
}
if (res == E1.OK) {
ApplyGrabDelay(_SubSysInfo);
return SequenceAction.Ok();
}
else {
return SequenceAction.Error(E2.ABORTED);
}
}
private string CheckFound(
SequenceContext context,
string id,
ICollection<string> branches,
SequenceActionResult param) {
var patternResult = param.Param as PatternResult;
if (patternResult.Status != eFoundStatus.NotFound) {
if (patternResult.Status == eFoundStatus.ForceOK) {
_IsForceOk = true;
}
return SEQ_ID_SET_MEASURE;
}
return SEQ_ID_CHECK_MEASURE_FINISHED;
}
private SequenceActionResult MarkNG(
SequenceContext context,
string id,
SequenceActionResult param) {
context.PutValue<AlignResult>(
string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()),
new AlignResult() {
AlignName = _AlignRecipe.AlignName,
Status = AlignStatus.NOT_FOUND,
AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
});
return SequenceAction.Ok();
}
#region 패턴 간에 거리 검사
private string CheckPatternDistanceInspEnabled(
SequenceContext context,
string id,
ICollection<string> branches,
SequenceActionResult param) {
if (_AlignRecipe.InspParams.PatternDistanceParam.Enabled) {
return SEQ_ID_SET_PATTERN_DISTANCE_INSP_SEQ_PARAMS;
}
else {
return SEQ_ID_ALIGN;
}
}
private SequenceActionResult SetPatternDistanceInspSeqParams(
SequenceContext context,
string id,
SequenceActionResult param) {
return SequenceAction.Ok(
new InspSeqParams_PatternDistance(_Params.AlignInfo, _MeasureResults));
}
private string CheckPatternDistanceInspStatus(
SequenceContext context,
string id,
ICollection<string> branches,
SequenceActionResult param) { // Param : List<PatternDistanceResult>
var patternDistanceResults = param.Param as List<PatternDistanceResult>;
foreach (PatternDistanceResult result in patternDistanceResults) {
if (result.InspStatus == InspStatus.NG) {
return SEQ_ID_DISTANCE_NG;
}
}
return SEQ_ID_ALIGN;
}
private SequenceActionResult DistanceNG(
SequenceContext context,
string id,
SequenceActionResult param) {
context.PutValue<AlignResult>(
string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()),
new AlignResult() {
AlignName = _AlignRecipe.AlignName,
Status = AlignStatus.DISTANCE_NG,
AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
});
return SequenceAction.Ok();
}
#endregion
private SequenceActionResult Align(
SequenceContext context,
string id,
SequenceActionResult param) {
var meaPoints = new List<DoublePoint>();
var refPoints = new List<DoublePoint>();
_MeasureResults = _MeasureResults.OrderBy(obj => obj.Key).ToDictionary(obj => obj.Key, obj => obj.Value);
foreach (var meaResultPair in _MeasureResults) {
meaPoints.Add((meaResultPair.Value as BaseResult).MaterialPosition);
foreach (var refResultPair in _ReferenceResults) {
if (refResultPair.Key == meaResultPair.Key) {
refPoints.Add((refResultPair.Value as BaseResult).MaterialPosition);
break;
}
}
}
var flipY = (int)_Calibrator.GetFlipY();
var alignResult = _Aligner.Align(meaPoints, refPoints, flipY);
if (alignResult == null) {
return SequenceAction.Error();
}
else {
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()), alignResult);
}
return SequenceAction.Ok(alignResult);
}
private SequenceActionResult SeqComplete(
SequenceContext context,
string id,
SequenceActionResult param) {
AlignResult alignResult = context.GetValue<AlignResult>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, _Params.AlignInfo.GetKey()));
if (alignResult == null) {
return SequenceAction.Error();
}
alignResult.AlignName = _AlignRecipe.AlignName;
if (_IsForceOk && alignResult.Status == AlignStatus.OK) {
alignResult.Status = AlignStatus.F_OK;
}
if (_MeasureResults.Keys.Count > 0) {
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_LAST_MEASURE_FOV_ID, _Params.AlignInfo.GetKey()), _MeasureResults.Keys.ToList().Max());
}
return SequenceAction.Ok(alignResult);
}
private void IncreaseCurFovId() {
_CurFovId++;
}
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) {
}
}
}