using A032.Process.Calibration;
|
using Bro.Common.Base;
|
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, 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, 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 < 1)
|
{
|
throw new ProcessException("请至少配置1个标定点位");
|
}
|
|
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++)
|
//{
|
// 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.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);
|
// }
|
//}
|
|
//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),
|
// });
|
//}
|
|
return new ProcessResponse(true);
|
}
|
|
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);
|
|
List<double> matrix = GetMovementMatrix(calibConfig.Configs.Select(u => u.ImageMarkPoint as CustomizedPoint).ToList(), calibConfig.Configs.Select(u => u.CurrentPlatPoint).ToList(), out string msg);
|
|
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 },
|
});
|
}
|
|
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)
|
//{
|
// throw new ProcessException("未能获取绑定设备信息");
|
//}
|
|
//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();
|
}
|
}
|
