From 6428fad15dbe79e30a48ffc9aabe31e03a45426c Mon Sep 17 00:00:00 2001
From: patrick <patrick.xu@broconcentric.com>
Date: 星期四, 05 十二月 2019 12:50:13 +0800
Subject: [PATCH] 1. 添加部分log信息 2. 修改部分机器人动作流程 3. 开放OperationConfig的InputData参数
---
src/A032.Process/ProcessControl_Calibration.cs | 372 ++++++++++++++++++++++++++++++++++++++++++++--------
1 files changed, 314 insertions(+), 58 deletions(-)
diff --git a/src/A032.Process/ProcessControl_Calibration.cs b/src/A032.Process/ProcessControl_Calibration.cs
index 49fa9ee..44a19ea 100644
--- a/src/A032.Process/ProcessControl_Calibration.cs
+++ b/src/A032.Process/ProcessControl_Calibration.cs
@@ -3,99 +3,355 @@
using Bro.Common.Helper;
using Bro.Common.Interface;
using Bro.Common.Model;
+using HalconDotNet;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
+using System.Threading;
using System.Threading.Tasks;
+using static Bro.Common.Helper.EnumHelper;
namespace A032.Process
{
public partial class ProcessControl
{
+ CalibReplyMsg _calibReply = new CalibReplyMsg();
+
[ProcessMethod("CalibrationCollection", "RobotCalibration", "鏈哄櫒浜�9鐐规爣瀹�", true)]
- public ProcessResponse RobotCalibration(IOperationConfig config)
+ public ProcessResponse RobotCalibration(IOperationConfig config, IDevice device)
{
+ CalibrationConfigCollection calibConfig = config as CalibrationConfigCollection;
+
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.CameraId == calibConfig.CameraId);
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鑾峰彇缁戝畾璁惧淇℃伅");
+ }
+
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == calibConfig.PositionNo);
+ if (position == null)
+ {
+ throw new ProcessException("鏈兘鑾峰彇姝ｇ‘浣嶇疆淇℃伅");
+ }
+
+ if (calibConfig.Configs.Count < 9)
+ {
+ throw new ProcessException("璇疯嚦灏戦厤缃�9涓爣瀹氱偣浣�");
+ }
+
+ if (calibConfig.IsStartedFromUI)
+ {
+ FrmCalib9PDynamic frm = new FrmCalib9PDynamic(this, calibConfig, bind, position, SendMessageToRobot_Calibration, CalculateMatrix);
+ frm.ShowDialog();
+ }
+ else
+ {
+ MultipleStepsProcess(calibConfig, bind, SendMessageToRobot_Calibration);
+
+ CalculateMatrix(calibConfig, bind, position);
+ }
+
+ //for (int i = 0; i < calibConfig.Configs.Count; i++)
+ //{
+ // bind.Robot.SendMsg(Bro.Device.AuboRobot.RobotMsgAction.Calibration, Bro.Device.AuboRobot.RobotMsgParas.None, calibConfig.PositionNo, new List<float>() { i + 1 });
+
+ // _calibReply.CalibHandle.WaitOne();
+
+ // if (_calibReply.CalibIndex != (i + 1) || _calibReply.CalibPositionNo != calibConfig.PositionNo)
+ // {
+ // throw new ProcessException("鏍囧畾鍙嶉鐨勭储寮曟垨浣嶇疆淇℃伅涓嶄竴鑷�");
+ // }
+
+ // calibConfig.Configs[i].CurrentPlatPoint = new CustomizedPoint(_calibReply.RobotPosition.X, _calibReply.RobotPosition.Y);
+
+ // using (HObject hImage = CollectHImage(bind.Camera, calibConfig.Configs[i].CameraOpConfig, "RobotCalibration"))
+ // {
+ // var tool = _halconToolDict[calibConfig.Configs[i].CameraOpConfig.AlgorithemPath];
+
+ // tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ // tool.RunProcedure();
+
+ // float x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ // float y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ // float angel = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ // if (x < 0 || y < 0)
+ // {
+ // throw new ProcessException("鑾峰彇鐐逛綅淇℃伅涓嶆纭�");
+ // }
+
+ // calibConfig.Configs[i].ImageMarkPoint = new CustomizedPointWithAngle(x, y, angel);
+ // }
+ //}
+
+ //HOperatorSet.VectorToHomMat2d(new HTuple(calibConfig.Configs.Select(u => u.ImageMarkPoint.X).ToArray()), new HTuple(calibConfig.Configs.Select(u => u.ImageMarkPoint.Y).ToArray()), new HTuple(calibConfig.Configs.Select(u => u.CurrentPlatPoint.X).ToArray()), new HTuple(calibConfig.Configs.Select(u => u.CurrentPlatPoint.Y).ToArray()), out HTuple matrix);
+
+ //var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == position.PositionCode);
+ //if (visionConfig != null)
+ //{
+ // visionConfig.Matrix = new List<double>() { matrix[0], matrix[1], 0, matrix[3], matrix[4], 0 };
+ //}
+ //else
+ //{
+ // Config.VisionConfigCollection.Add(new PositionVisionConfig()
+ // {
+ // CameraId = bind.CameraId,
+ // PositionCode = position.PositionCode,
+ // Matrix = new List<double>() { matrix[0], matrix[1], 0, matrix[3], matrix[4], 0 },
+ // });
+ //}
+
return new ProcessResponse(true);
}
[ProcessMethod("CalibrationCollection", "StandardPointCalibration", "鏍囧噯鐐逛綅鏍囧畾", true)]
- public ProcessResponse StandardPointCalibration(IOperationConfig config)
+ public ProcessResponse StandardPointCalibration(IOperationConfig config, IDevice device)
{
- return new ProcessResponse(true);
- }
+ CalibrationConfigCollection calibConfig = config as CalibrationConfigCollection;
- public void SendCalibrationStartSignal(int stepNum)
- {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.CameraId == calibConfig.CameraId);
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鑾峰彇缁戝畾璁惧淇℃伅");
+ }
- }
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == calibConfig.PositionNo);
+ if (position == null)
+ {
+ throw new ProcessException("鏈兘鑾峰彇姝ｇ‘浣嶇疆淇℃伅");
+ }
- private int MultipleStepsCalibration(CalibrationConfigCollection configs, Action<List<CalibrationConfig>> FinalCalculation)
- {
- throw new NotImplementedException();
+ if (calibConfig.Configs.Count < 1)
+ {
+ throw new ProcessException("璇疯嚦灏戦厤缃�1涓爣瀹氱偣浣�");
+ }
- //configs.Configs.ForEach(c =>
+ if (calibConfig.IsStartedFromUI)
+ {
+ FrmCalib9PDynamic frm = new FrmCalib9PDynamic(this, calibConfig, bind, position, SendMessageToRobot_Standard, CalculateStandardPoint);
+ frm.ShowDialog();
+ }
+ else
+ {
+ MultipleStepsProcess(calibConfig, bind, SendMessageToRobot_Standard);
+
+ CalculateStandardPoint(calibConfig, bind, position);
+ }
+
+ //for (int i = 0; i < calibConfig.Configs.Count; i++)
//{
- // c.Image = null;
- //});
+ // bind.Robot.SendMsg(Bro.Device.AuboRobot.RobotMsgAction.Calibration, Bro.Device.AuboRobot.RobotMsgParas.None, calibConfig.PositionNo, new List<float>() { i + 1 });
- //if (string.IsNullOrWhiteSpace(configs.CameraId) || !CameraDict.ContainsKey(configs.CameraId))
- //{
- // throw new ProcessException("鏍囧畾閰嶇疆鏈厤缃纭殑鐩告満缂栧彿");
- //}
+ // _calibReply.CalibHandle.WaitOne();
- //if (string.IsNullOrWhiteSpace(configs.PositionCode))
- //{
- // throw new ProcessException("鏍囧畾閰嶇疆鏈寚瀹氳矾寰勪綅缃�");
- //}
-
- //CameraBase camera = CameraDict[configs.CameraId];
- //FrmCalib9PDynamic frm9PDynamic = new FrmCalib9PDynamic(this, camera, configs, FinalCalculation);
- //frm9PDynamic.ShowDialog();
-
- //if (configs.InputPara == null || configs.InputPara.Count <= 0)
- //{
- // return (int)PLCReplyValue.NG;
- //}
-
- //if (configs.InputPara[0] <= 0 || configs.InputPara[0] > configs.Configs.Count)
- //{
- // configs.InputPara = null;
- // return (int)PLCReplyValue.IGNORE;
- //}
-
- //int sequence = configs.InputPara[0];
-
- ////绗竴娆�
- //if (sequence == 1)
- //{
- // configs.Configs.ForEach(c =>
+ // if (_calibReply.CalibPositionNo != calibConfig.PositionNo)
// {
- // c.Image = null;
- // c.OfflineImagePath = "";
- // c.CurrentPlatPoint = new CustomizedPoint();
- // c.ImageMarkPoint = new CustomizedPoint();
+ // throw new ProcessException("鏍囧畾鍙嶉鐨勪綅缃俊鎭笉涓�鑷�");
+ // }
+
+ // calibConfig.Configs[i].CurrentPlatPoint = new CustomizedPoint(_calibReply.RobotPosition.X, _calibReply.RobotPosition.Y);
+
+ // using (HObject hImage = CollectHImage(bind.Camera, calibConfig.Configs[i].CameraOpConfig, "RobotCalibration"))
+ // {
+ // var tool = _halconToolDict[calibConfig.Configs[i].CameraOpConfig.AlgorithemPath];
+
+ // tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ // tool.RunProcedure();
+
+ // float x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ // float y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ // float angel = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ // if (x < 0 || y < 0)
+ // {
+ // throw new ProcessException("鑾峰彇鐐逛綅淇℃伅涓嶆纭�");
+ // }
+
+ // calibConfig.Configs[i].ImageMarkPoint = new CustomizedPointWithAngle(x, y, angel);
+ // }
+ //}
+
+ //CustomizedPointWithAngle markPoint = calibConfig.Configs[0].ImageMarkPoint;
+ //var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == position.PositionCode);
+ //if (visionConfig != null)
+ //{
+ // visionConfig.StandardPoint = new CustomizedPointWithAngle(markPoint.X, markPoint.Y, markPoint.Angle);
+ //}
+ //else
+ //{
+ // Config.VisionConfigCollection.Add(new PositionVisionConfig()
+ // {
+ // CameraId = bind.CameraId,
+ // PositionCode = position.PositionCode,
+ // StandardPoint = new CustomizedPointWithAngle(markPoint.X, markPoint.Y, markPoint.Angle),
// });
//}
- //CalibrationConfig stepConfig = configs.Configs[sequence - 1];
+ return new ProcessResponse(true);
+ }
- //HDevEngineTool tool = _halconToolDict[]
+ public void CalculateMatrix(CalibrationConfigCollection calibConfig, AGVBindUnit bind, PathPosition position)
+ {
+ //HOperatorSet.VectorToHomMat2d(new HTuple(calibConfig.Configs.Select(u => u.ImageMarkPoint.X).ToArray()), new HTuple(calibConfig.Configs.Select(u => u.ImageMarkPoint.Y).ToArray()), new HTuple(calibConfig.Configs.Select(u => u.CurrentPlatPoint.X).ToArray()), new HTuple(calibConfig.Configs.Select(u => u.CurrentPlatPoint.Y).ToArray()), out HTuple matrix);
- //CalibMarkPoint(camera, stepConfig, sequence);
+ List<double> matrix = GetMovementMatrix(calibConfig.Configs.Select(u => u.ImageMarkPoint as CustomizedPoint).ToList(), calibConfig.Configs.Select(u => u.CurrentPlatPoint).ToList(), out string msg);
- ////鑾峰彇褰撳墠骞冲彴鐐逛綅
- //stepConfig.CurrentPlatPoint = new CustomizedPoint(_monitorList.Skip(locationStartIndex).Take(4).ToList());
- ////stepConfig.CurrentPlatPoint = new CustomizedPoint(configs.InputPara.Skip(1).Take(4).ToList());
+ var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == position.PositionCode);
+ if (visionConfig != null)
+ {
+ visionConfig.Matrix = new List<double>() { matrix[0], matrix[1], 0, matrix[3], matrix[4], 0 };
+ }
+ else
+ {
+ Config.VisionConfigCollection.Add(new PositionVisionConfig()
+ {
+ CameraId = bind.CameraId,
+ PositionCode = position.PositionCode,
+ Matrix = new List<double>() { matrix[0], matrix[1], 0, matrix[3], matrix[4], 0 },
+ });
+ }
- ////鏈�鍚庝竴娆�
- //if (sequence == configs.Configs.Count)
+ PubSubCenter.Publish(PubTag.CalibAllDone.ToString(), "", msg, true);
+ }
+
+ /// <summary>
+ /// 鑾峰彇鏍囧畾鐐圭殑杞崲鐭╅樀
+ /// </summary>
+ /// <param name="points"></param>
+ /// <returns></returns>
+ protected List<double> GetMovementMatrix(List<CustomizedPoint> imagePoints, List<CustomizedPoint> platPoints, out string msg)
+ {
+ HTuple uList, vList, xList, yList, matrix;
+
+ ConvertPointToHTuple(imagePoints, out uList, out vList);
+ ConvertPointToHTuple(platPoints, out xList, out yList);
+
+ HOperatorSet.VectorToHomMat2d(uList, vList, xList, yList, out matrix);
+
+ double sum = 0;
+ for (int i = 0; i < imagePoints.Count; i++)
+ {
+ HOperatorSet.AffineTransPoint2d(matrix, imagePoints[i].X, imagePoints[i].Y, out HTuple m, out HTuple n);
+
+ sum += Math.Sqrt((Math.Pow((m.D - platPoints[i].X), 2) + Math.Pow((n.D - platPoints[i].Y), 2)));
+ }
+
+ //sum = ((sum / (double)Config.LengthPulseRatio) * 100.0) / ((double)imagePoints.Count);
+ sum = sum / (double)imagePoints.Count;
+
+ msg = $"鏍囧畾鐐规暟閲�:{imagePoints.Count};鍗曠偣璇樊:{sum.ToString()}鑴夊啿";
+
+ return matrix.DArr.ToList();
+ }
+
+ private void ConvertPointToHTuple(List<CustomizedPoint> point, out HTuple x, out HTuple y)
+ {
+ x = new HTuple(point.Select(p => p.X).ToArray());
+ y = new HTuple(point.Select(p => p.Y).ToArray());
+ }
+
+ public void CalculateStandardPoint(CalibrationConfigCollection calibConfig, AGVBindUnit bind, PathPosition position)
+ {
+ //var bind = Config.AGVBindCollection.FirstOrDefault(u => u.CameraId == calibConfig.CameraId);
+ //if (bind == null)
//{
- // FinalCalculation?.Invoke(configs.Configs);
+ // throw new ProcessException("鏈兘鑾峰彇缁戝畾璁惧淇℃伅");
//}
- //configs.InputPara = null;
- //return (int)PLCReplyValue.OK;
+ //PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == calibConfig.PositionNo);
+ //if (position == null)
+ //{
+ // throw new ProcessException("鏈兘鑾峰彇姝ｇ‘浣嶇疆淇℃伅");
+ //}
+
+ CustomizedPointWithAngle markPoint = calibConfig.Configs[0].ImageMarkPoint;
+ var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == position.PositionCode);
+ if (visionConfig != null)
+ {
+ visionConfig.StandardPoint = new CustomizedPointWithAngle(markPoint.X, markPoint.Y, markPoint.Angle);
+ }
+ else
+ {
+ Config.VisionConfigCollection.Add(new PositionVisionConfig()
+ {
+ CameraId = bind.CameraId,
+ PositionCode = position.PositionCode,
+ StandardPoint = new CustomizedPointWithAngle(markPoint.X, markPoint.Y, markPoint.Angle),
+ });
+ }
+
+ PubSubCenter.Publish(PubTag.CalibAllDone.ToString(), "", $"鏍囧畾瀹屾垚锛屾爣鍑嗙偣:{markPoint.GetDisplayText()}", true);
}
+
+ public void MultipleStepsProcess(CalibrationConfigCollection calibConfig, AGVBindUnit bind, Action<AGVBindUnit, int, int> sendMessageToRobot)
+ {
+ for (int i = 0; i < calibConfig.Configs.Count; i++)
+ {
+ SingleStepProcess(calibConfig.Configs[i], sendMessageToRobot, bind, calibConfig.PositionNo, i);
+ }
+ }
+
+ public void SendMessageToRobot_Calibration(AGVBindUnit bind, int positionNo, int index)
+ {
+ bind.Robot.SendMsg(Bro.Device.AuboRobot.RobotMsgAction.Calibration, Bro.Device.AuboRobot.RobotMsgParas.None, positionNo, new List<float>() { index + 1 });
+
+ _calibReply.CalibHandle.WaitOne();
+
+ if (_calibReply.CalibIndex != (index + 1) || _calibReply.CalibPositionNo != positionNo)
+ {
+ throw new ProcessException("鏍囧畾鍙嶉鐨勭储寮曟垨浣嶇疆淇℃伅涓嶄竴鑷�");
+ }
+ }
+
+ public void SendMessageToRobot_Standard(AGVBindUnit bind, int positionNo, int index)
+ {
+ bind.Robot.SendMsg(Bro.Device.AuboRobot.RobotMsgAction.StandardPoint, Bro.Device.AuboRobot.RobotMsgParas.None, positionNo);
+
+ _calibReply.CalibHandle.WaitOne();
+
+ if (_calibReply.CalibPositionNo != positionNo)
+ {
+ throw new ProcessException("鏍囧畾鍙嶉鐨勪綅缃俊鎭笉涓�鑷�");
+ }
+ }
+
+ //PubSubCenter.Subscribe(PubTag.CalibStepDone.ToString(), CalibStepDone);
+ //PubSubCenter.Subscribe(PubTag.CalibAllDone.ToString(), CalibAllDone);
+ public void SingleStepProcess(CalibrationConfig config, Action<AGVBindUnit, int, int> sendMessageToRobot, AGVBindUnit bind, int positionNo, int index)
+ {
+ sendMessageToRobot.Invoke(bind, positionNo, index);
+
+ config.CurrentPlatPoint = new CustomizedPoint(_calibReply.RobotPosition.X, _calibReply.RobotPosition.Y);
+
+ using (HObject hImage = CollectHImage(bind.Camera, config.CameraOpConfig, "RobotCalibration"))
+ {
+ var tool = _halconToolDict[config.CameraOpConfig.AlgorithemPath];
+
+ tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ tool.RunProcedure();
+
+ float x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ float y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ float angel = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ if (x < 0 || y < 0)
+ {
+ throw new ProcessException("鑾峰彇鐐逛綅淇℃伅涓嶆纭�");
+ }
+
+ config.ImageMarkPoint = new CustomizedPointWithAngle(x, y, angel);
+ }
+
+ PubSubCenter.Publish(PubTag.CalibStepDone.ToString(), index, "", true);
+ }
+ }
+
+ public class CalibReplyMsg
+ {
+ public AutoResetEvent CalibHandle { get; set; } = new AutoResetEvent(false);
+
+ public int CalibIndex { get; set; } = 0;
+
+ public int CalibPositionNo { get; set; }
+
+ public CustomizedPoint RobotPosition { get; set; } = new CustomizedPoint();
}
}
--
Gitblit v1.8.0