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

522 lines
22 KiB
C#

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_SPN_AN 얼라인 시퀀스 파라미터
/// </summary>
public class AlignSeqParams_C1_F1_SPN_AN : 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_SPN_AN 얼라인 시퀀스 파라미터를 얻어온다.
/// </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_SPN_AN(
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개 / 패턴 1개 / 얼라인 N개 / 1 Point Align N개 / MeasurePattern to (Center + Target Offset)
/// </summary>
public class AlignSeq_C1_F1_SPN_AN : SubSeqBase {
private readonly Logger LOG = new Logger(typeof(AlignSeq_C1_F1_SPN_AN).Name);
private AlignSeqParams_C1_F1_SPN_AN _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 List<int> _PatternIndex = new List<int>();
private string _SeqName;
private int _FindNumber = 0;
#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_FIND = "FIND";
private const string SEQ_ID_CHECK_FOUND = "CHECK_FOUND";
private const string SEQ_ID_ALIGN = "ALIGN";
private const string SEQ_ID_ALIGN_EXIT = "ALIGN_EXIT";
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_SPN_AN() {
}
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.AddAction(SEQ_ID_FIND, Find);
sb.AddDecision(SEQ_ID_CHECK_FOUND, CheckFound);
sb.AddAction(SEQ_ID_ALIGN, Align);
sb.AddAction(SEQ_ID_ALIGN_EXIT, AlignExit);
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_FIND]
> sb[SEQ_ID_CHECK_FOUND]
> sb[SEQ_ID_ALIGN]
> sb[SEQ_ID_ALIGN_EXIT]
> sb[SEQ_ID_UPDATE_UI]
> sb[SEQ_ID_SEQ_COMPLETE]
);
sb.Flows(
sb[SEQ_ID_CHECK_FOUND] // Measure NG
> sb[SEQ_ID_ALIGN_EXIT]
);
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_SPN_AN;
}
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 = FovPatternAgentManager.GetAgent(_SubSysInfo);
_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);
if (!InitProcData()) {
return SequenceAction.Error();
}
return SequenceAction.Ok();
}
private bool InitProcData() {
var procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
//PatternInfo 에서 FindNumber를 가져온다.
var patternInfo = GetPatternInfo(_Params.AlignInfo, _FovInfo, procPatternInfo);
_FindNumber = _PatternAgent.GetFindNumber(patternInfo);
//PatternInfo의 FindNumber와 RefDist의 수가 다른 경우 AlignSeq를 멈춘다.
if (_FindNumber != _Params.RefDistParam.RefDists.Count) {
return false;
}
for (int patternPosIdx = 0; patternPosIdx < _Params.RefDistParam.RefDists.Count; patternPosIdx++) {
if (!_MeasurePatternResults.ContainsKey(patternPosIdx)) {
_MeasurePatternResults.Add(patternPosIdx, null);
}
else {
_MeasurePatternResults[patternPosIdx] = null;
}
if (!_AlignResults.ContainsKey(patternPosIdx)) {
_AlignResults.Add(patternPosIdx, null);
}
else {
_AlignResults[patternPosIdx] = null;
}
if (!_MeasureResults.ContainsKey(patternPosIdx)) {
_MeasureResults.Add(patternPosIdx, null);
}
else {
_MeasureResults[patternPosIdx] = null;
}
}
return true;
}
private SequenceActionResult Clear(
SequenceContext context,
string id,
SequenceActionResult param) {
return SequenceAction.Ok();
}
private SequenceActionResult SetFixture(
SequenceContext context,
string id,
SequenceActionResult param) {
var procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
var patternInfo = GetPatternInfo(_Params.AlignInfo, _FovInfo, procPatternInfo);
PatternIndex fixturePatternIndex = null;
if (patternInfo.FixtureSetting.Enabled) {
fixturePatternIndex = FixtureSetting.GetFixturePatternIndex(patternInfo.GetPatternIndex(), patternInfo.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;
procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, _AlignRecipe.AlignId.ToString(), eProcPatternId.ProcPattern1);
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) {
return SEQ_ID_ALIGN;
}
else {
// Mark NG
var alignResult = new AlignResult() {
Status = AlignStatus.NOT_FOUND,
Delta = new AlignDelta(),
AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
};
for (int i = 0; i < _Params.RefDistParam.RefDists.Count; i++) {
alignResult.AlignName = $"Align{i}";
_AlignResults[i] = alignResult;
}
return SEQ_ID_ALIGN_EXIT;
}
}
private void SetMeasurePatterResults(string id, PatternResult patternResult) {
if (id == SEQ_ID_CHECK_FOUND) {
var fovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfo);
List<StaticPoint> refDists = new List<StaticPoint>();
for (int i = 0; i < _Params.RefDistParam.RefDists.Count; i++) {
//refDist와 FovCenter 사이의 거리를 저장하여 FovCenter가 원점이 아닐 경우에도 계산이 되도록 한다.
var refPoint = new StaticPoint(_Params.RefDistParam.RefDists[i].DistX, _Params.RefDistParam.RefDists[i].DistY) + fovRecipe.CalFovRecipe.FovCenter;
refDists.Add(new StaticPoint(refPoint.X, refPoint.Y));
}
List<StaticPoint> patternPoints = new List<StaticPoint>();
var baseResults = patternResult.Results.ToList();
foreach (var result in baseResults) {
StaticPoint staticResult = result.MaterialPosition;
patternPoints.Add(staticResult);
}
_PatternIndex = PatternUtil.GetNearestPointIndex(patternPoints, refDists, _Params.RefDistParam.DistanceTolerance);
//Pattern의 Index가 나온 경우 Index에 맞게 PatternResult를 MeasurePatternResults에 저장한다.
for (int i = 0; i < _PatternIndex.Count; i++) {
if (_PatternIndex[i] != -1) {
_MeasureResults[_PatternIndex[i]] = baseResults[i];
List<BaseResult> newResults = new List<BaseResult>();
newResults.Add(baseResults[i]);
PatternResult newPatternResult = new PatternResult() {
PatternIndex = patternResult.PatternIndex.Clone() as PatternIndex,
PatternType = patternResult.PatternType,
PatternName = patternResult.PatternName,
Status = patternResult.Status,
Results = newResults,
FindRoi = patternResult.FindRoi.Clone() as ROI,
TactTimeInMilliSec = patternResult.TactTimeInMilliSec,
MoreInfo = patternResult.MoreInfo,
FovId = patternResult.FovId,
};
_MeasurePatternResults[_PatternIndex[i]] = newPatternResult;
}
}
//Pattern의 Index가 나오지 않은 경우 해당 Index의 패턴을 NotFound 상태로 저장한다.
for (int i = 0; i < _Params.RefDistParam.RefDists.Count; i++) {
if (_MeasurePatternResults[i] == null) {
BaseResult baseResult = new BaseResult() {
ImagePosition = new ImagePoint(),
MaterialPosition = new StaticPoint(),
Angle = new Angle(),
};
List<BaseResult> newResults = new List<BaseResult>();
newResults.Add(baseResult);
_MeasurePatternResults[i] = new PatternResult() {
PatternIndex = patternResult.PatternIndex.Clone() as PatternIndex,
PatternType = patternResult.PatternType,
PatternName = patternResult.PatternName,
Status = eFoundStatus.NotFound,
Results = newResults,
FindRoi = patternResult.FindRoi.Clone() as ROI,
TactTimeInMilliSec = 0,
MoreInfo = patternResult.MoreInfo,
FovId = patternResult.FovId,
};
}
}
}
}
private SequenceActionResult Align(
SequenceContext context,
string id,
SequenceActionResult param) {
for (int i = 0; i < _Params.RefDistParam.RefDists.Count; i++) {
if (_MeasureResults[i] == null) {
var alignResult = new AlignResult() {
Status = AlignStatus.NOT_FOUND,
Delta = new AlignDelta(),
AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm,
};
alignResult.AlignName = $"Align{(i)}";
_AlignResults[i] = alignResult;
BaseResult baseResult = new BaseResult() {
ImagePosition = new ImagePoint(),
MaterialPosition = new StaticPoint(),
Angle = new Angle(),
};
_MeasureResults[i] = baseResult;
}
else {
var baseResult = _MeasureResults[i];
var meaPoint = baseResult.MaterialPosition;
var refPoint = _Params.ReferenceResults[0];
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 = i;
alignResult.DisplayType = _Params.DisplayType;
alignResult.AlignName = $"Align{(i)}";
alignResult.AlignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm;
_AlignResults[i] = 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() {
Dictionary<int, PatternResult> cloneMeasurePatternResults = _MeasurePatternResults.ToDictionary(x => x.Key, x => x.Value?.Clone());
// 얼라인 탭에 패턴 및 얼라인 로그 기록
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(subSysInfo.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) {
}
}
}