2026-01-26 18:56:53 +09:00

523 lines
20 KiB
C#

/*
* Copyright (c) 2025 LG Electronics Inc.
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
*/
using Aurora.Api;
using Cognex.VisionPro;
using Cognex.VisionPro.Display;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Windows.Forms;
using System.Windows;
using System.Windows.Input;
using Aurora;
using VisionCalibrator;
using VisionCommon;
using CogCommon;
using Aurora.Api.DataType;
using System.Threading.Tasks;
using Aurora.Api.Devices;
using VisionDirector;
using Aurora.View;
namespace CogUI {
public class CogRuler {
private const string KEY_INTERACTIVE_POSITION = "INTERACTIVE_POSITION";
private const string KEY_INTERACTIVE_LABEL = "INTERACTIVE_LABEL";
private const string KEY_INTERACTIVE_LINE = "INTERACTIVE_LINE";
private const string KEY_INTERACTIVE_TEMP_LABEL = "INTERACTIVE_TEMP_LABEL";
private const string KEY_INTERACTIVE_TEMP_LINE = "INTERACTIVE_TEMP_LINE";
private const string KEY_INTERACTIVE_CIRCLE = "INTERACTIVE_CIRCLE";
private const string KEY_INTERACTIVE_TEMP_CIRCLE = "INTERACTIVE_TEMP_CIRCLE";
private Logger _Logger = new Logger("CogRuler");
private CogDisplay _CogDisplay;
private CameraBase _CameraDev;
private FovInfo _FovInfo;
private FovRecipe _FovRecipe;
private Calibrator _Calibrator;
private MotionDirector _teachingDirector;
private List<CogColorConstants> _colors;
private int _colorIndex = 0;
private StaticPoint _pixelSize;
private ImageSize _imageSize;
private ImagePoint _imageCenter;
private bool _isMoving = false;
private ImagePoint _StartPoint;
private ImagePoint _EndPoint;
private int _crossLength = 30;
private bool _IsMouseDown = false;
private object _Lock = new object();
public CogRuler(CogDisplay cogDisplay, FovInfo fovInfo) {
_Logger = new Logger("CogRuler");
_CogDisplay = cogDisplay;
_FovInfo = fovInfo;
_FovRecipe = VisionRecipeManager.GetFovRecipe(fovInfo);
_Calibrator = CalibratorManager.GetCalibrator(fovInfo);
_CameraDev = VisionEnv.AuroraEnv.DeviceManager[_FovRecipe.CamRecipe.CamId.ToString()] as CameraBase;
_colors = new List<CogColorConstants>() {
CogColorConstants.Yellow,
CogColorConstants.Magenta
};
_pixelSize = _FovRecipe.CalFovRecipe.PixelSize.Clone() as StaticPoint;
_teachingDirector = TeachingDirectorManager.Instance.GetDirector(fovInfo.ToSubSysInfo());
SetImageSize();
SetImageCenter();
}
private void SetImageSize() {
if (_CogDisplay.Image != null) {
_imageSize = CogEnvironment.GetImageSize(_CogDisplay.Image);
}
else {
var camConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(_FovRecipe.CamRecipe.CamId.ToString()) as CamConfig;
_imageSize = camConfig.ImageSize;
}
}
private void SetImageCenter() {
_imageCenter = new ImagePoint(_imageSize.Width * 0.5, _imageSize.Height * 0.5);
}
public void SetDistanceAngleMode() {
if (_CogDisplay == null) {
return;
}
Clear(true);
_CogDisplay.MouseDown += OnMouseDown_DistanceAndAngle;
_CogDisplay.MouseMove += OnMouseMove_DistanceAndAngle;
_CogDisplay.MouseUp += OnMouseUp_DistanceAndAngle;
}
public void SetPositionMode() {
if (_CogDisplay == null) {
return;
}
Clear(true);
_CogDisplay.MouseDown += OnMouseDown_Position;
}
public void SetCircleMode() {
if (_CogDisplay == null) {
return;
}
Clear(true);
_CogDisplay.MouseDown += OnMouseDown_Circle;
_CogDisplay.MouseMove += OnMouseMove_Circle;
_CogDisplay.MouseUp += OnMouseUp_Circle;
}
public void SetMovePositionMode() {
if (_CogDisplay == null) {
return;
}
Clear(true);
_CogDisplay.MouseDown += OnMouseDown_MovePosition;
}
#region 1.Distance and angle
private void OnMouseDown_DistanceAndAngle(object sender, System.Windows.Forms.MouseEventArgs e) {
if (e.Button == MouseButtons.Left) {
_IsMouseDown = true;
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
_StartPoint = imagePoint;
DisplayDrawer.AddCross(
_CogDisplay, GetColor(),
new Cross(_StartPoint.Clone() as ImagePoint, _crossLength),
KEY_INTERACTIVE_POSITION);
}
}
private void OnMouseMove_DistanceAndAngle(object sender, System.Windows.Forms.MouseEventArgs e) {
if ((e.Button == MouseButtons.Left) && _IsMouseDown) {
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
_EndPoint = imagePoint;
if (IsSamePosition(_StartPoint, _EndPoint)) {
return;
}
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LINE);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LABEL);
AddLineSegment(_StartPoint, _EndPoint);
AddLabel_DistanceAndAngle(_StartPoint, _EndPoint, KEY_INTERACTIVE_TEMP_LABEL);
}
}
private void OnMouseUp_DistanceAndAngle(object sender, System.Windows.Forms.MouseEventArgs e) {
if (e.Button == MouseButtons.Left && _IsMouseDown) {
_IsMouseDown = false;
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
_EndPoint = imagePoint;
if (IsSamePosition(_StartPoint, _EndPoint)) {
return;
}
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LINE);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LABEL);
DisplayDrawer.AddCross(
_CogDisplay, GetColor(),
new Cross(_EndPoint.Clone() as ImagePoint, _crossLength),
KEY_INTERACTIVE_POSITION);
AddLineSegment(_StartPoint, _EndPoint);
AddLabel_DistanceAndAngle(_StartPoint, _EndPoint, KEY_INTERACTIVE_LABEL);
ChangeColor();
}
}
private bool IsOutOfImageArea(ImagePoint point) {
double width = _imageSize.Width;
double height = _imageSize.Height;
if ((0 <= point.X && point.X < width) && (0 <= point.Y && point.Y < height)) {
return false;
}
else {
return true;
}
}
private bool IsSamePosition(ImagePoint startPoint, ImagePoint endPoint) {
if ((startPoint.X == endPoint.X) && (startPoint.Y == endPoint.Y)) {
return true;
}
else {
return false;
}
}
public ImagePoint TransformScreenPointToImagePoint(ImagePoint src) {
string screenSpace = "*";
string imageSpace = "@";
var transform = _CogDisplay.GetTransform(imageSpace, screenSpace);
double transformedX, transformedY;
transform.MapPoint(src.X, src.Y, out transformedX, out transformedY);
return new ImagePoint(transformedX, transformedY);
}
private void AddLineSegment(ImagePoint startPoint, ImagePoint endPoint) {
string groupName = (_IsMouseDown) ? KEY_INTERACTIVE_TEMP_LINE : KEY_INTERACTIVE_LINE;
DisplayDrawer.AddLine(_CogDisplay, GetColor(), startPoint, endPoint, groupName);
DisplayDrawer.AddCross(
_CogDisplay, GetColor(),
new Cross(startPoint.Clone() as ImagePoint, _crossLength),
KEY_INTERACTIVE_TEMP_LINE);
DisplayDrawer.AddCross(
_CogDisplay, GetColor(),
new Cross(endPoint.Clone() as ImagePoint, _crossLength),
KEY_INTERACTIVE_TEMP_LINE);
}
private void AddLabel_DistanceAndAngle(ImagePoint startPoint, ImagePoint endPoint, string groupName) {
lock (_Lock) {
var msgs = new List<string>();
// Distance [pxl]
var distancePxl = MathUtil.CalculateDistance(startPoint, endPoint);
var distancePxlStr = string.Format("{0:F1} [pxl]", distancePxl);
msgs.Add(distancePxlStr);
// Distance [mm]
var imageSize = CogEnvironment.GetImageSize(_CogDisplay.Image);
var staticStartPoint = _Calibrator.TransformToCalibratedSpace(_StartPoint, imageSize);
var staticEndPoint = _Calibrator.TransformToCalibratedSpace(_EndPoint, imageSize);
var distance = MathUtil.CalculateDistance(staticStartPoint, staticEndPoint);
var distanceStr = string.Format("{0:F3} [mm]", distance);
msgs.Add(distanceStr);
// Angle [deg]
var angle = MathUtil.GetLineAngle(startPoint, endPoint);
var angleDegStr = string.Format("{0:F4} [deg]", angle.Degree);
msgs.Add(angleDegStr);
DisplayDrawer.AddTextList(_CogDisplay, GetColor(), endPoint, msgs, groupName);
}
}
#endregion
#region 2.Position
private void OnMouseDown_Position(object sender, System.Windows.Forms.MouseEventArgs e) {
if (e.Button == MouseButtons.Left) {
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
AddPosition(imagePoint);
ChangeColor();
}
}
private void AddPosition(ImagePoint point) {
lock (_Lock) {
DisplayDrawer.AddCross(
_CogDisplay, GetColor(),
new Cross(point.Clone() as ImagePoint, _crossLength),
KEY_INTERACTIVE_POSITION);
AddPositionText(GetColor(), point);
}
}
private void AddPositionText(CogColorConstants color, ImagePoint imagePosition) {
var imageSize = CogEnvironment.GetImageSize(_CogDisplay.Image);
ImagePoint imageCenter = new ImagePoint(imageSize.Width / 2, imageSize.Height / 2);
ImagePoint imageOffsetFromCenter = (imagePosition - imageCenter) as ImagePoint;
var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_FovInfo);
var calibrator = CalibratorManager.GetCalibrator(_FovInfo);
var staticPosition = calibrator.TransformToCalibratedSpace(imagePosition, imageSize);
var imageCenterStaticPosition = calibrator.TransformToCalibratedSpace(imageCenter, imageSize);
StaticPoint staticOffset = (staticPosition - imageCenterStaticPosition) as StaticPoint;
var msgs = new List<string>() {
string.Format($"I.P({imagePosition.X:0.00}, {imagePosition.Y:0.00})"),
string.Format($"I.O({imageOffsetFromCenter.X:0.00}, {imageOffsetFromCenter.Y:0.00})"),
string.Format($"S.P({staticPosition.X:0.00}, {staticPosition.Y:0.00})"),
string.Format($"S.O({staticOffset.X:0.00}, {staticOffset.Y:0.00})")
};
DisplayDrawer.AddTextList(
_CogDisplay, color, imagePosition, msgs, KEY_INTERACTIVE_LABEL);
}
#endregion
#region 3.Circle
private void OnMouseDown_Circle(object sender, System.Windows.Forms.MouseEventArgs e) {
if (e.Button == MouseButtons.Left) {
_IsMouseDown = true;
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
_StartPoint = imagePoint;
}
}
private void OnMouseMove_Circle(object sender, System.Windows.Forms.MouseEventArgs e) {
if ((e.Button == MouseButtons.Left) && _IsMouseDown) {
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
_EndPoint = imagePoint;
if (IsSamePosition(_StartPoint, _EndPoint)) {
return;
}
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_CIRCLE);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LABEL);
var pixelRadius = MathUtil.CalculateDistance(_StartPoint, _EndPoint);
DisplayDrawer.AddCircle(_CogDisplay, GetColor(), _StartPoint, pixelRadius, KEY_INTERACTIVE_TEMP_CIRCLE);
AddLabel_Circle(_StartPoint, _EndPoint, pixelRadius, KEY_INTERACTIVE_TEMP_LABEL);
}
}
private void OnMouseUp_Circle(object sender, System.Windows.Forms.MouseEventArgs e) {
if (e.Button == MouseButtons.Left && _IsMouseDown) {
_IsMouseDown = false;
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (IsOutOfImageArea(imagePoint)) {
return;
}
_EndPoint = imagePoint;
if (IsSamePosition(_StartPoint, _EndPoint)) {
return;
}
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_CIRCLE);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LABEL);
var pixelRadius = MathUtil.CalculateDistance(_StartPoint, _EndPoint);
DisplayDrawer.AddCircle(_CogDisplay, GetColor(), _StartPoint, pixelRadius, KEY_INTERACTIVE_CIRCLE);
AddLabel_Circle(_StartPoint, _EndPoint, pixelRadius, KEY_INTERACTIVE_LABEL);
ChangeColor();
}
}
private void AddLabel_Circle(ImagePoint startPoint, ImagePoint endPoint, double pixelRadius, string groupName) {
lock (_Lock) {
var staticRadius = GetDistance(startPoint, endPoint, _pixelSize);
var msgs = new List<string>() {
string.Format("R : {0:0.0000} [px]", pixelRadius),
string.Format("R : {0:0.0000} [mm]", staticRadius),
string.Format("D : {0:0.0000} [px]", pixelRadius * 2),
string.Format("D : {0:0.0000} [mm]", staticRadius * 2)
};
DisplayDrawer.AddTextList(_CogDisplay, GetColor(), endPoint, msgs, groupName);
}
}
private double GetDistance(ImagePoint p1, ImagePoint p2, StaticPoint scale) {
return Math.Sqrt(
Math.Pow((p1.X - p2.X) * scale.X, 2) +
Math.Pow((p1.Y - p2.Y) * scale.Y, 2));
}
#endregion
#region 4.MovePosition
private void OnMouseDown_MovePosition(object sender, System.Windows.Forms.MouseEventArgs e) {
if (e.Button == MouseButtons.Left) {
lock (_Lock) {
if (IsMoving()) {
MessageUtil.ShowErrorToast("Motor is Moving");
return;
}
SetIsMoving(true);
}
var screenPoint = new ImagePoint(e.X, e.Y);
var imagePoint = TransformScreenPointToImagePoint(screenPoint);
if (!IsOutOfImageArea(imagePoint)) {
var centerStaticPoint = _Calibrator.TransformToCalibratedSpace(_imageCenter, _imageSize);
var targetStaticPoint = _Calibrator.TransformToCalibratedSpace(imagePoint, _imageSize);
var moveOffset = targetStaticPoint - centerStaticPoint;
moveOffset = _teachingDirector.ApplyDirection(moveOffset);
_Logger.I($"{imagePoint}, {moveOffset}");
RelMove(moveOffset);
_StartPoint = imagePoint;
}
else {
SetIsMoving(false);
}
}
}
private bool IsMoving() {
lock (_Lock) {
return _isMoving;
}
}
private void SetIsMoving(bool isMovsing) {
lock (_Lock) {
_isMoving = isMovsing;
}
}
private async void RelMove(DoublePoint moveOffset) {
await Task.Run(() => {
EquipAdapter.Instance.TeachingMotionManager.RelMove(_FovInfo, new Position() { X = moveOffset.X, Y = moveOffset.Y });
SetIsMoving(false);
});
}
#endregion
public CogColorConstants GetColor() {
return _colors[_colorIndex];
}
public void ChangeColor() {
_colorIndex++;
if (_colors.Count <= _colorIndex) {
_colorIndex = 0;
}
}
#region Clear UI
public void Clear(bool isEventClear) {
if (_CogDisplay == null) {
return;
}
ClearDistanceAndAngle(isEventClear);
ClearPosition(isEventClear);
ClearCircle(isEventClear);
ClearMovePosition(isEventClear);
}
public void ClearDistanceAndAngle(bool isEventClear) {
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_POSITION);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_LABEL);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_LINE);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LABEL);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LINE);
if (isEventClear) {
_CogDisplay.MouseDown -= OnMouseDown_DistanceAndAngle;
_CogDisplay.MouseMove -= OnMouseMove_DistanceAndAngle;
_CogDisplay.MouseUp -= OnMouseUp_DistanceAndAngle;
}
}
public void ClearPosition(bool isEventClear) {
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_POSITION);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_LABEL);
if (isEventClear) {
_CogDisplay.MouseDown -= OnMouseDown_Position;
}
}
public void ClearCircle(bool isEventClear) {
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_LABEL);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_CIRCLE);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_LABEL);
DisplayDrawer.RemoveInteractiveGraphic(_CogDisplay, KEY_INTERACTIVE_TEMP_CIRCLE);
if (isEventClear) {
_CogDisplay.MouseDown -= OnMouseDown_Circle;
_CogDisplay.MouseMove -= OnMouseMove_Circle;
_CogDisplay.MouseUp -= OnMouseUp_Circle;
}
}
public void ClearMovePosition(bool isEventClear) {
if (isEventClear) {
_CogDisplay.MouseDown -= OnMouseDown_MovePosition;
}
}
#endregion
}
}