411 lines
15 KiB
C#
411 lines
15 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using VisionCommon;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using Aurora.Api.Devices;
|
|
using Hermes;
|
|
using VisionSequence.Pattern;
|
|
using System.Linq;
|
|
|
|
namespace VisionSequence.Setup {
|
|
/// <summary>
|
|
/// 캘리브레이션 서브 시퀀스 요청 파라미터
|
|
/// </summary>
|
|
public class CalSubSeqParams {
|
|
/// <summary>FOV 키 정보 리스트</summary>
|
|
public List<FovInfo> FovInfos { get; set; }
|
|
/// <summary>메인 화면 타입</summary>
|
|
public eDisplayType DisplayType { get; set; }
|
|
/// <summary>캘리브레이션 플랜아이템 리스트</summary>
|
|
public List<PlanItem> PlanItems { get; set; }
|
|
/// <summary>카메라 대수</summary>
|
|
public int CamCnt { get; set; }
|
|
//public Action<PlanItem> CallBack { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 서브 시퀀스 결과 파라미터
|
|
/// </summary>
|
|
public class CalSubSeqResultParams {
|
|
/// <summary>X축 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
|
|
public List<List<ImagePoint>> XPoints;
|
|
/// <summary>Y축 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
|
|
public List<List<ImagePoint>> YPoints;
|
|
/// <summary>회전 원점 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
|
|
public List<List<ImagePoint>> ThetaPoints;
|
|
|
|
/// <summary>X축으로 1회 이동 거리</summary>
|
|
public List<double> LengthX;
|
|
/// <summary>Y축으로 1회 이동 거리</summary>
|
|
public List<double> LengthY;
|
|
/// <summary>T축으로 1회 회전 각도(Degree)</summary>
|
|
public List<double> LengthTheta;
|
|
/// <summary>패턴 검출 NG 발생 플래그</summary>
|
|
public bool IsPatternNotFound;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 픽셀 사이즈 및 회전 원점 계산
|
|
/// </summary>
|
|
public class SetupSeq_CalSub : SubSeqBase {
|
|
private readonly Logger LOG = new Logger(typeof(SetupSeq_CalSub).Name);
|
|
|
|
private CalSubSeqParams _Params;
|
|
private ImageManager _ImageManager = new ImageManager();
|
|
|
|
private SubSysInfo _SubSysInfo;
|
|
private List<FovInfo> _FovInfos;
|
|
private List<FovRecipe> _FovRecipes;
|
|
|
|
private int _CamCnt;
|
|
private string _SeqName;
|
|
|
|
private int _PlanCurrentStep;
|
|
private int _SubseqRepeatCnt;
|
|
private bool _IsPatternFound;
|
|
|
|
private List<List<ImagePoint>> _XPoints;
|
|
private List<List<ImagePoint>> _YPoints;
|
|
private List<List<ImagePoint>> _ThetaPoints;
|
|
|
|
private List<double> _LengthX;
|
|
private List<double> _LengthY;
|
|
private List<double> _LengthTheta;
|
|
|
|
#region Sequence Id
|
|
private const string SEQ_ID_INIT = "INIT";
|
|
private const string SEQ_ID_PREPARE = "PREPARE";
|
|
private const string SEQ_ID_MOVE_TEACH_POS = "MOVE_TEACH_POS";
|
|
private const string SEQ_ID_MOVE = "MOVE";
|
|
private const string SEQ_ID_CLEAR = "CLEAR";
|
|
private const string SEQ_ID_SET_GRAB_FIND_SEQ_PARAMS = "SET_GRAB_FIND_SEQ_PARAMS";
|
|
private const string SEQ_ID_GRAB_FIND_SEQ = "GRAB_FIND_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_SEQ_COMPLETE = "SEQ_COMPLETE";
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
public SetupSeq_CalSub() {
|
|
}
|
|
|
|
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_PREPARE]
|
|
> sb[SEQ_ID_MOVE_TEACH_POS]
|
|
> sb[SEQ_ID_MOVE]
|
|
> sb[SEQ_ID_CLEAR]
|
|
> sb[SEQ_ID_SET_GRAB_FIND_SEQ_PARAMS]
|
|
> sb[SEQ_ID_GRAB_FIND_SEQ]
|
|
> sb[SEQ_ID_SAVE_IMAGE]
|
|
> sb[SEQ_ID_APPLY_DATA]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_MOVE]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_CLEAR]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
> sb.Final
|
|
);
|
|
|
|
#if SEQ_DRAW
|
|
sb.Draw($"{GetType().Name}.pdf");
|
|
#endif
|
|
|
|
return sb;
|
|
}
|
|
|
|
private void SetActions(SequenceBuilder sb) {
|
|
var grabFindSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindSeq(), "GrabFindSeq");
|
|
|
|
sb.AddAction(SEQ_ID_INIT, Init);
|
|
sb.AddAction(SEQ_ID_PREPARE, Prepare);
|
|
sb.AddAction(SEQ_ID_MOVE_TEACH_POS, MoveTeachPos);
|
|
sb.AddAction(SEQ_ID_MOVE, Move);
|
|
sb.AddAction(SEQ_ID_CLEAR, Clear);
|
|
sb.AddAction(SEQ_ID_SET_GRAB_FIND_SEQ_PARAMS, SetGrabFindSeqParams);
|
|
sb.AddSubsequence(SEQ_ID_GRAB_FIND_SEQ, grabFindSeqBuilder);
|
|
sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
|
|
sb.AddDecision(SEQ_ID_APPLY_DATA, ApplyData);
|
|
sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
|
|
}
|
|
|
|
private SequenceActionResult Init(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (param.Param != null) {
|
|
_Params = param.Param as CalSubSeqParams;
|
|
}
|
|
|
|
_SubSysInfo = _Params.FovInfos[0].ToSubSysInfo();
|
|
_FovInfos = _Params.FovInfos;
|
|
_CamCnt = _Params.CamCnt;
|
|
|
|
_FovRecipes = new List<FovRecipe>();
|
|
_PlanCurrentStep = 0;
|
|
|
|
_XPoints = new List<List<ImagePoint>>();
|
|
_YPoints = new List<List<ImagePoint>>();
|
|
_ThetaPoints = new List<List<ImagePoint>>();
|
|
|
|
_LengthX = new List<double>();
|
|
_LengthY = new List<double>();
|
|
_LengthTheta = new List<double>();
|
|
|
|
for (int i = 0; i < _CamCnt; i++) {
|
|
_FovRecipes.Add(VisionRecipeManager.GetFovRecipe(_FovInfos[i]));
|
|
_XPoints.Add(new List<ImagePoint>());
|
|
_YPoints.Add(new List<ImagePoint>());
|
|
_ThetaPoints.Add(new List<ImagePoint>());
|
|
}
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult Prepare(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (_FovRecipes != null)
|
|
return SequenceAction.Ok();
|
|
else
|
|
return SequenceAction.Error();
|
|
}
|
|
|
|
private SequenceActionResult MoveTeachPos(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfos[0], ePositionType.Teaching);
|
|
if (res == E1.OK) {
|
|
Thread.Sleep(100);
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult Clear(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
GrabFindSeqUtil.Clear(_SubSysInfo);
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult Move(SequenceContext context, string id, SequenceActionResult param) {
|
|
var item = _Params.PlanItems[_PlanCurrentStep].Clone();
|
|
_SubseqRepeatCnt = 0;
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
Thread.Sleep(200);
|
|
return SequenceAction.Ok(item);
|
|
}
|
|
|
|
// Image -> Motion Direction
|
|
var csSource = new CoordSystem(); // Image CS
|
|
var alignDirectorRecipe = VisionRecipeManager.GetAlignDirectorRecipe(_SubSysInfo);
|
|
var csTarget = alignDirectorRecipe.CoordSystem;
|
|
var moveXY = CoordSystem.ApplyDirection(
|
|
new MotionPoint(double.Parse(item.X), double.Parse(item.Y)),
|
|
csSource, csTarget);
|
|
var moveT = CoordSystem.ApplyDirectionT(double.Parse(item.T), csTarget);
|
|
var motionDelta = new Position() {
|
|
X = moveXY.X,
|
|
Y = moveXY.Y,
|
|
T = moveT
|
|
};
|
|
|
|
// Move
|
|
EquipAdapter.Instance.AlignMotionManager.RelMove(_SubSysInfo, motionDelta);
|
|
Thread.Sleep(500);
|
|
|
|
return SequenceAction.Ok(item);
|
|
}
|
|
|
|
private SequenceActionResult SetGrabFindSeqParams(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
FovInfo fovInfo = _FovInfos[_SubseqRepeatCnt];
|
|
|
|
var fovRecipe = VisionRecipeManager.GetFovRecipe(fovInfo);
|
|
var patternAgent = PatternAgentManager.GetAgent(fovInfo);
|
|
var patternIndex = new PatternIndex(fovInfo, fovRecipe.FovName, fovRecipe.CalFovRecipe.PatternId);
|
|
|
|
var patternInfo = patternAgent.GetPatternInfo(patternIndex);
|
|
eSeqMode seqMode = eSeqMode.Manual; // 그랩을 Skip하지 않기 위함.
|
|
|
|
return SequenceAction.Ok(new GrabFindSeqParams(fovInfo, patternInfo, _Params.DisplayType, seqMode));
|
|
}
|
|
|
|
private SequenceActionResult SaveImage(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
|
|
FovInfo fovInfo = _FovInfos[_SubseqRepeatCnt];
|
|
|
|
var display = CameraDisplayManager.GetDisplay(fovInfo, _Params.DisplayType);
|
|
object image = display.GetImageObject();
|
|
|
|
if (image != null) {
|
|
_ImageManager.SaveImageInternal(fovInfo, eSaveDataType.Calibration, image);
|
|
}
|
|
}
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
|
|
return SequenceAction.Ok(patternResult);
|
|
}
|
|
|
|
private string ApplyData(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
|
|
List<ImagePoint> patternResultList = new List<ImagePoint>();
|
|
List<StaticPoint> staticResultList = new List<StaticPoint>();
|
|
List<Angle> resultAngleList = new List<Angle>();
|
|
FovInfo fovInfo = _FovInfos[_SubseqRepeatCnt];
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
if (patternResult.Status == eFoundStatus.FoundOK) //하나 이상일 경우 OK
|
|
{
|
|
_IsPatternFound = true;
|
|
foreach (var result in patternResult.Results) {
|
|
var position = result.ImagePosition.Clone() as ImagePoint;
|
|
patternResultList.Add(position);
|
|
|
|
var staticPosition = result.MaterialPosition.Clone() as StaticPoint;
|
|
staticResultList.Add(staticPosition);
|
|
|
|
var angle = result.Angle.Clone() as Angle;
|
|
resultAngleList.Add(angle);
|
|
}
|
|
}
|
|
else if (patternResult.Status != eFoundStatus.FoundOK) {
|
|
_IsPatternFound = false;
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
|
|
_Params.PlanItems[_PlanCurrentStep].FovId = (int)fovInfo.FovId;
|
|
var item = _Params.PlanItems[_PlanCurrentStep].Clone();
|
|
|
|
item.ImagePosX = patternResultList[0].X.ToString("F4");
|
|
item.ImagePosY = patternResultList[0].Y.ToString("F4");
|
|
|
|
item.StaticPosX = staticResultList[0].X.ToString("F4");
|
|
item.StaticPosY = staticResultList[0].Y.ToString("F4");
|
|
|
|
item.ResultT = resultAngleList[0].Degree.ToString("F4");
|
|
|
|
//Log
|
|
context.EventPublisher.Publish(new EventMessage("SetupSeq_Cal", VisionEventId.UPDATE_CAL_SUB_SEQ_LOG, item));
|
|
|
|
if (_PlanCurrentStep + 1 == _Params.PlanItems.Count) {
|
|
if (_SubseqRepeatCnt + 1 == _Params.CamCnt) {
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
}
|
|
else if (item.Purpose == Constants.X_CAL) {
|
|
SaveCalDataForPurpose(item, context, "X");
|
|
}
|
|
else if (item.Purpose == Constants.Y_CAL) {
|
|
SaveCalDataForPurpose(item, context, "Y");
|
|
}
|
|
else if (item.Purpose == Constants.THETA_CAL) {
|
|
SaveCalDataForPurpose(item, context, "T");
|
|
}
|
|
|
|
_SubseqRepeatCnt++;
|
|
if (_SubseqRepeatCnt == _Params.CamCnt) {
|
|
_PlanCurrentStep++;
|
|
return SEQ_ID_MOVE;
|
|
}
|
|
return SEQ_ID_CLEAR;
|
|
}
|
|
|
|
private void SaveCalDataForPurpose(PlanItem item, SequenceContext context, string purpose) {
|
|
if (purpose == "X") {
|
|
if (item.Action == Constants.MOVE) {
|
|
_LengthX.Add(double.Parse(item.X));
|
|
_XPoints[_SubseqRepeatCnt].Add(new ImagePoint(double.Parse(item.ImagePosX), double.Parse(item.ImagePosY)));
|
|
}
|
|
}
|
|
else if (purpose == "Y") {
|
|
if (item.Action == Constants.MOVE) {
|
|
_LengthY.Add(double.Parse(item.Y));
|
|
_YPoints[_SubseqRepeatCnt].Add(new ImagePoint(double.Parse(item.ImagePosX), double.Parse(item.ImagePosY)));
|
|
}
|
|
}
|
|
else {
|
|
if (item.Action == Constants.MOVE) {
|
|
_LengthTheta.Add(double.Parse(item.T));
|
|
_ThetaPoints[_SubseqRepeatCnt].Add(new ImagePoint(double.Parse(item.ImagePosX), double.Parse(item.ImagePosY)));
|
|
}
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult SeqComplete(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
CalSubSeqResultParams returnParam = new CalSubSeqResultParams();
|
|
if (_IsPatternFound) {
|
|
returnParam.XPoints = _XPoints;
|
|
returnParam.YPoints = _YPoints;
|
|
returnParam.ThetaPoints = _ThetaPoints;
|
|
|
|
returnParam.LengthX = _LengthX;
|
|
returnParam.LengthY = _LengthY;
|
|
returnParam.LengthTheta = _LengthTheta;
|
|
returnParam.IsPatternNotFound = false;
|
|
}
|
|
else {
|
|
returnParam.IsPatternNotFound = true;
|
|
}
|
|
|
|
return SequenceAction.Ok(returnParam);
|
|
}
|
|
|
|
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) {
|
|
}
|
|
}
|
|
}
|