414 lines
16 KiB
C#
414 lines
16 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using System.Collections.Generic;
|
|
using Aurora.Api;
|
|
using VisionCommon;
|
|
using Hermes;
|
|
using System.Threading;
|
|
using VisionSequence.Pattern;
|
|
using VisionCommonUI;
|
|
using VisionCommonUI.Resources;
|
|
using System.Linq;
|
|
using Aurora.Api.Devices;
|
|
|
|
namespace VisionSequence.Setup {
|
|
/// <summary>
|
|
/// 축 반복도 측정 셋업 시퀀스 요청 파라미터
|
|
/// </summary>
|
|
public class SetupSeqParams_DynamicRepeat {
|
|
/// <summary>FOV 키 정보</summary>
|
|
public FovInfo FovInfo { get; set; }
|
|
/// <summary>메인 화면 타입</summary>
|
|
public eDisplayType DisplayType { get; set; }
|
|
/// <summary>모터 이동 반복 횟수</summary>
|
|
public int MotionRepeatCount { get; set; }
|
|
/// <summary>축 반복도 측정 반복 횟수</summary>
|
|
public int VisionRepeatCount { get; set; }
|
|
/// <summary>패턴 정보</summary>
|
|
public PatternInfo PatternInfo { get; set; }
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="fovInfo">FOV 키 정보</param>
|
|
/// <param name="patternInfo">패턴 정보</param>
|
|
/// <param name="motionRepeatCount">모터 이동 반복 횟수</param>
|
|
/// <param name="visionRepeatCount">축 반복도 측정 반복 횟수</param>
|
|
/// <param name="displayType">메인 화면 타입</param>
|
|
public SetupSeqParams_DynamicRepeat(FovInfo fovInfo, PatternInfo patternInfo,
|
|
int motionRepeatCount = 1,
|
|
int visionRepeatCount = 1,
|
|
eDisplayType displayType = eDisplayType.VisionRepeat) {
|
|
|
|
this.FovInfo = fovInfo;
|
|
this.DisplayType = displayType;
|
|
this.MotionRepeatCount = motionRepeatCount;
|
|
this.VisionRepeatCount = visionRepeatCount;
|
|
this.PatternInfo = patternInfo;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 축 반복도 측정
|
|
/// </summary>
|
|
public class SetupSeq_DynamicRepeat {
|
|
private readonly Logger LOG = new Logger(typeof(SetupSeq_DynamicRepeat).Name);
|
|
private SetupSeqParams_DynamicRepeat _Params;
|
|
private FovRecipe _FovRecipe;
|
|
private FovInfo _FovInfo;
|
|
private bool _IsPatternNotFound;
|
|
private CsvManager _CsvManager = new CsvManager();
|
|
private ImageManager _ImageManager = new ImageManager();
|
|
|
|
private List<ImagePoint> _ImagePoints = new List<ImagePoint>();
|
|
private List<StaticPoint> _StaticPoints = new List<StaticPoint>();
|
|
|
|
#region Sequence Id
|
|
private const string SEQ_ID_INIT = "INIT";
|
|
private const string SEQ_ID_CHECK_PATTERN = "CHECK_PATTERN";
|
|
private const string SEQ_ID_UNDEFINED_PATTERN_NG = "SEQ_ID_UNDEFINED_PATTERN_NG";
|
|
private const string SEQ_ID_PREPARE = "PREPARE";
|
|
private const string SEQ_ID_MOV_READY_POS = "MOVE_READY_POSITION";
|
|
private const string SEQ_ID_MOV_TEACH_POS = "MOVE_TEACH_POSITION";
|
|
private const string SEQ_ID_CLEAR = "CLEAR";
|
|
private const string SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS = "SET_GRAB_FIND_PROC_SEQ_PARAMS";
|
|
private const string SEQ_ID_GRAB_FIND_PROC_SEQ = "GRAB_FIND_PROC_SEQ";
|
|
private const string SEQ_ID_SAVE_IMAGE = "SEQ_ID_SAVE_IMAGE";
|
|
private const string SEQ_ID_APPLY_DATA = "ApplyData";
|
|
private const string SEQ_ID_MOV_FINAL_POS = "MOVE_FINAL_POSITION";
|
|
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
|
|
#endregion
|
|
|
|
#region key
|
|
const string KEY_PATTERN_FOUND = "KEY_PATTERN_FOUND";
|
|
const string KEY_CURRENT_MOTION_STEP = "KEY_PLAN_CURRENT_MOTION_STEP";
|
|
const string KEY_CURRENT_VISION_STEP = "KEY_PLAN_CURRENT_VISION_STEP";
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
public SetupSeq_DynamicRepeat() {
|
|
}
|
|
|
|
public SequenceBuilder Builder() {
|
|
SequenceBuilder sb = new SequenceBuilder(1000);
|
|
SetActions(sb);
|
|
sb.OnStopped(OnStopped);
|
|
sb.OnForceError(OnForceError);
|
|
|
|
sb.Flows(
|
|
sb.Init
|
|
> sb[SEQ_ID_INIT]
|
|
> sb[SEQ_ID_CHECK_PATTERN]
|
|
> sb[SEQ_ID_PREPARE]
|
|
> sb[SEQ_ID_MOV_READY_POS]
|
|
> sb[SEQ_ID_MOV_TEACH_POS]
|
|
> sb[SEQ_ID_CLEAR]
|
|
> sb[SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS]
|
|
> sb[SEQ_ID_GRAB_FIND_PROC_SEQ]
|
|
> sb[SEQ_ID_SAVE_IMAGE]
|
|
> sb[SEQ_ID_APPLY_DATA]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_CHECK_PATTERN]
|
|
> sb[SEQ_ID_UNDEFINED_PATTERN_NG]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_MOV_READY_POS]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_CLEAR]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_MOV_FINAL_POS]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
> sb.Final
|
|
);
|
|
|
|
#if SEQ_DRAW
|
|
sb.Draw($"{GetType().Name}.pdf");
|
|
#endif
|
|
|
|
return sb;
|
|
}
|
|
|
|
private void SetActions(SequenceBuilder sb) {
|
|
SequenceBuilder grabFindProcSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindProcSeq(), "GrabFindProcSeq");
|
|
|
|
sb.AddAction(SEQ_ID_INIT, Init);
|
|
sb.AddDecision(SEQ_ID_CHECK_PATTERN, CheckPattern);
|
|
sb.AddAction(SEQ_ID_UNDEFINED_PATTERN_NG, UndefinedPatternNG);
|
|
sb.AddAction(SEQ_ID_PREPARE, Prepare);
|
|
sb.AddAction(SEQ_ID_MOV_READY_POS, MoveReadyPos);
|
|
sb.AddAction(SEQ_ID_MOV_TEACH_POS, MoveTeachPos);
|
|
sb.AddAction(SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS, SetGrabFindProcSeqParams);
|
|
sb.AddSubsequence(SEQ_ID_GRAB_FIND_PROC_SEQ, grabFindProcSeqBuilder);
|
|
sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
|
|
sb.AddDecision(SEQ_ID_APPLY_DATA, ApplyData);
|
|
sb.AddAction(SEQ_ID_CLEAR, Clear);
|
|
sb.AddAction(SEQ_ID_MOV_FINAL_POS, MoveReadyPos);
|
|
sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
|
|
}
|
|
|
|
private SequenceActionResult Init(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (param.Param != null) {
|
|
_Params = param.Param as SetupSeqParams_DynamicRepeat;
|
|
}
|
|
|
|
_FovRecipe = VisionRecipeManager.GetFovRecipe(_Params.FovInfo);
|
|
_FovInfo = _Params.FovInfo;
|
|
|
|
context.PutValue(KEY_CURRENT_MOTION_STEP, 0);
|
|
context.PutValue(KEY_CURRENT_VISION_STEP, 0);
|
|
_ImagePoints.Clear();
|
|
_StaticPoints.Clear();
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private string CheckPattern(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
|
|
var visionRepeatRecipe = VisionRecipeManager.GetVisionRepeatRecipe(_Params.FovInfo);
|
|
if (visionRepeatRecipe.PatternId == -1) {
|
|
_IsPatternNotFound = true;
|
|
return SEQ_ID_UNDEFINED_PATTERN_NG;
|
|
}
|
|
|
|
return SEQ_ID_PREPARE;
|
|
}
|
|
|
|
private SequenceActionResult UndefinedPatternNG(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
string errMsg = "Undefined PatternID Available";
|
|
LOG.E(errMsg);
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult Prepare(SequenceContext context, string id, SequenceActionResult param) {
|
|
//if (_VsRps != null) {
|
|
// if (_VsRps.UnitType == eUnitType.FlexTransfer) {
|
|
// int res = AbsMoveSub(_axesAvoid, _moverAvoid);
|
|
// if (res != E1.OK) {
|
|
// return SequenceAction.Error(E2.ABORTED);
|
|
// }
|
|
// }
|
|
//}
|
|
//else {
|
|
// return SequenceAction.Error();
|
|
//}
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult MoveReadyPos(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.Ready);
|
|
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult MoveTeachPos(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.MasterGlass);
|
|
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult Clear(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
GrabFindSeqUtil.Clear(_Params.FovInfo.ToSubSysInfo());
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult SetGrabFindProcSeqParams(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
var visionRepeatRecipe = VisionRecipeManager.GetVisionRepeatRecipe(_Params.FovInfo);
|
|
eSeqMode seqMode = eSeqMode.Manual; // 그랩을 Skip하지 않기 위함.
|
|
|
|
return SequenceAction.Ok(new GrabFindProcSeqParams(_Params.FovInfo, _Params.PatternInfo, _Params.DisplayType, seqMode));
|
|
}
|
|
|
|
private SequenceActionResult SaveImage(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
|
|
var display = CameraDisplayManager.GetDisplay(_FovInfo, _Params.DisplayType);
|
|
object image = display.GetImageObject();
|
|
|
|
if (image != null) {
|
|
_ImageManager.SaveImageInternal(_FovInfo, eSaveDataType.VisionRepeat, image);
|
|
}
|
|
}
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
|
|
return SequenceAction.Ok(patternResult);
|
|
}
|
|
|
|
private string ApplyData(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
|
|
ImagePoint imagePoint = new ImagePoint();
|
|
StaticPoint staticPoint = new StaticPoint();
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
if (patternResult.Status == eFoundStatus.FoundOK) //하나 이상일 경우 OK
|
|
{
|
|
context.PutValue<bool>(KEY_PATTERN_FOUND, true);
|
|
var result = patternResult.Results.ToList()[0];
|
|
imagePoint = result.ImagePosition.Clone() as ImagePoint;
|
|
staticPoint = result.MaterialPosition.Clone() as StaticPoint;
|
|
|
|
_ImagePoints.Add(imagePoint);
|
|
_StaticPoints.Add(staticPoint);
|
|
|
|
int curr_vision = 1 + context.GetValue<int>(KEY_CURRENT_VISION_STEP);
|
|
context.PutValue(KEY_CURRENT_VISION_STEP, curr_vision);
|
|
if (context.GetValue<int>(KEY_CURRENT_VISION_STEP) < _Params.VisionRepeatCount) {
|
|
return SEQ_ID_CLEAR;
|
|
}
|
|
else {
|
|
int curr_motion = 1 + context.GetValue<int>(KEY_CURRENT_MOTION_STEP);
|
|
context.PutValue(KEY_CURRENT_MOTION_STEP, curr_motion);
|
|
context.PutValue(KEY_CURRENT_VISION_STEP, 0);
|
|
|
|
UpdateVisionRepeatLogs(curr_motion, result);
|
|
|
|
if (context.GetValue<int>(KEY_CURRENT_MOTION_STEP) == _Params.MotionRepeatCount) {
|
|
return SEQ_ID_MOV_FINAL_POS;
|
|
}
|
|
else {
|
|
Thread.Sleep(300);
|
|
return SEQ_ID_MOV_READY_POS;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
context.PutValue<bool>(KEY_PATTERN_FOUND, false);
|
|
return SEQ_ID_MOV_FINAL_POS;
|
|
}
|
|
}
|
|
|
|
private void UpdateVisionRepeatLogs(int step, BaseResult result) {
|
|
var visionRepeatRecipe = VisionRecipeManager.GetVisionRepeatRecipe(_Params.FovInfo);
|
|
|
|
if (_Params.PatternInfo.PatternType == ePatternType.ShapeModelFinder) {
|
|
var shapeResult = result as ShapeModelFinderResult;
|
|
var imagePosition = shapeResult.ImagePosition.Clone();
|
|
var materialPosition = shapeResult.MaterialPosition.Clone();
|
|
|
|
var log = new VisionRepeatLog(
|
|
step, imagePosition.X, imagePosition.Y,
|
|
materialPosition.X, materialPosition.Y, shapeResult.Angle.Degree,
|
|
shapeResult.Width, shapeResult.Height, shapeResult.Radius);
|
|
|
|
VisionEnv.AuroraEnv.EventPublisher.Publish(new EventMessage(
|
|
_Params.FovInfo.GetKey(), VisionEventId.UPDATE_VISION_REPEAT_LOG, log));
|
|
|
|
_CsvManager.SaveCsv(_FovInfo.ToSubSysInfo(), VisionRepeatResultItemManager.GetVisionRepeatLog(_FovInfo.ToSubSysInfo(), log), eSaveDataType.VisionRepeat);
|
|
}
|
|
|
|
else {
|
|
var baseResult = result.Clone();
|
|
var log = new VisionRepeatLog(
|
|
step, baseResult.ImagePosition.X, baseResult.ImagePosition.Y,
|
|
baseResult.MaterialPosition.X, baseResult.MaterialPosition.Y,
|
|
baseResult.Angle.Degree);
|
|
|
|
VisionEnv.AuroraEnv.EventPublisher.Publish(new EventMessage(
|
|
_Params.FovInfo.GetKey(), VisionEventId.UPDATE_VISION_REPEAT_LOG, log));
|
|
|
|
_CsvManager.SaveCsv(_FovInfo.ToSubSysInfo(), VisionRepeatResultItemManager.GetVisionRepeatLog(_FovInfo.ToSubSysInfo(), log), eSaveDataType.VisionRepeat);
|
|
}
|
|
}
|
|
|
|
//private SequenceActionResult MoveFinalPos(SequenceContext context, string id, SequenceActionResult param) {
|
|
// if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
// return SequenceAction.Ok();
|
|
// }
|
|
|
|
// int res = MotionManager.MovePosition(_Params.FovInfo, _Params.FovInfo.FovId, ePositionType.Ready);
|
|
|
|
// if (res == E1.OK) {
|
|
// return SequenceAction.Ok();
|
|
// }
|
|
// else {
|
|
// return SequenceAction.Error(E2.ABORTED);
|
|
// }
|
|
//}
|
|
|
|
private SequenceActionResult SeqComplete(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
if (_IsPatternNotFound) {
|
|
return SequenceAction.Ok(new SetupSeqResultParams_VisionRepeat(_ImagePoints, _StaticPoints));
|
|
}
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok(new SetupSeqResultParams_VisionRepeat(_ImagePoints, _StaticPoints));
|
|
}
|
|
else {
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.Ready);
|
|
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok(new SetupSeqResultParams_VisionRepeat(_ImagePoints, _StaticPoints));
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
}
|
|
|
|
void OnStopped(SequenceContext context) {
|
|
LOG.I($"{context.SequenceId} - STOP Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void OnForceError(SequenceContext context, string id) {
|
|
LOG.I($"{context.SequenceId} - FORCE ERROR Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void HandleAbnormalFinish(SequenceContext context) {
|
|
}
|
|
}
|
|
}
|