1. 重新整理项目，按照A034模式，将设备异步操作修改为类同步操作。使用任务队列来存储和分配任务。

patrick

2019-12-06 b0a4c47dd74bc41c5df3bab6ddd8de7bcc6a52b0

 src/A032.Process/ProcessControl_Calibration.cs

@@ -3,6 +3,7 @@
using Bro.Common.Helper;
using Bro.Common.Interface;
using Bro.Common.Model;
using Bro.Device.AuboRobot;
using HalconDotNet;
using System;
using System.Collections.Generic;
@@ -16,8 +17,6 @@
{
    public partial class ProcessControl
    {
        CalibReplyMsg _calibReply = new CalibReplyMsg();

        [ProcessMethod("CalibrationCollection", "RobotCalibration", "机器人9点标定", true)]
        public ProcessResponse RobotCalibration(IOperationConfig config, IDevice device)
        {
@@ -29,7 +28,7 @@
                throw new ProcessException("未能获取绑定设备信息");
            }

            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == calibConfig.PositionNo);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionCode == calibConfig.PositionCode);
            if (position == null)
            {
                throw new ProcessException("未能获取正确位置信息");
@@ -42,64 +41,15 @@

            if (calibConfig.IsStartedFromUI)
            {
                FrmCalib9PDynamic frm = new FrmCalib9PDynamic(this, calibConfig, bind, position, SendMessageToRobot_Calibration, CalculateMatrix);
                FrmCalib9PDynamic frm = new FrmCalib9PDynamic(this, calibConfig, bind, position, CalculateMatrix);
                frm.ShowDialog();
            }
            else
            {
                MultipleStepsProcess(calibConfig, bind, SendMessageToRobot_Calibration);
                MultipleStepsProcess(calibConfig, bind);

                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);
        }
@@ -115,7 +65,7 @@
                throw new ProcessException("未能获取绑定设备信息");
            }

            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == calibConfig.PositionNo);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionCode == calibConfig.PositionCode);
            if (position == null)
            {
                throw new ProcessException("未能获取正确位置信息");
@@ -128,72 +78,22 @@

            if (calibConfig.IsStartedFromUI)
            {
                FrmCalib9PDynamic frm = new FrmCalib9PDynamic(this, calibConfig, bind, position, SendMessageToRobot_Standard, CalculateStandardPoint);
                FrmCalib9PDynamic frm = new FrmCalib9PDynamic(this, calibConfig, bind, position, CalculateStandardPoint);
                frm.ShowDialog();
            }
            else
            {
                MultipleStepsProcess(calibConfig, bind, SendMessageToRobot_Standard);
                MultipleStepsProcess(calibConfig, bind);

                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);
            List<double> matrix = GetMovementMatrix(calibConfig.Configs.Select(u => u.ImageMarkPoint as CustomizedPoint).ToList(), calibConfig.Configs.Select(u => new CustomizedPoint(u.PlatPoint.X, u.PlatPoint.Y)).ToList(), out string msg);

            var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == position.PositionCode);
            if (visionConfig != null)
@@ -235,7 +135,6 @@
                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()}脉冲";
@@ -251,18 +150,6 @@

        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)
@@ -282,51 +169,25 @@
            PubSubCenter.Publish(PubTag.CalibAllDone.ToString(), "", $"标定完成，标准点:{markPoint.GetDisplayText()}", true);
        }

        public void MultipleStepsProcess(CalibrationConfigCollection calibConfig, AGVBindUnit bind, Action<AGVBindUnit, int, int> sendMessageToRobot)
        public void MultipleStepsProcess(CalibrationConfigCollection calibConfig, AGVBindUnit bind)
        {
            for (int i = 0; i < calibConfig.Configs.Count; i++)
            {
                SingleStepProcess(calibConfig.Configs[i], sendMessageToRobot, bind, calibConfig.PositionNo, i);
                SingleStepProcess(calibConfig.Configs[i], bind, i);
            }
        }

        public void SendMessageToRobot_Calibration(AGVBindUnit bind, int positionNo, int index)
        public void SingleStepProcess(CalibrationConfig config, AGVBindUnit bind, 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);
            bind.Robot.Move(config.PlatPoint, MoveType.AbsoluteMove, true);

            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.InputImageDic.Clear();
                tool.InputImageDic["INPUT_Image"] = hImage;

                tool.RunProcedure();

                float x = (float)tool.GetResultTuple("OUTPUT_X").D;
@@ -342,16 +203,5 @@

            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();
    }
}