1. 重构项目

patrick

2019-12-10 1c4426810c71eead57084be8a18ade8d314dd8c4

 src/A032.Process/ProcessControl_Calibration.cs

@@ -1,101 +1,203 @@
using A032.Process.Calibration;
using Bro.Common.Base;
using Bro.Common.Helper;
using Bro.Common.Interface;
using Bro.Common.Model;
using Bro.Device.AuboRobot;
using HalconDotNet;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static Bro.Common.Helper.EnumHelper;

namespace A032.Process
{
    public partial class ProcessControl
    {
        [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.PositionCode == calibConfig.PositionCode);
            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, CalculateMatrix);
                frm.ShowDialog();
            }
            else
            {
                MultipleStepsProcess(calibConfig, bind);

                CalculateMatrix(calibConfig, bind, position);
            }

            return new ProcessResponse(true);
        }

        [ProcessMethod("CalibrationCollection", "StandardPointCalibration", "标准点位标定", true)]
        public ProcessResponse StandardPointCalibration(IOperationConfig config)
        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.PositionCode == calibConfig.PositionCode);
            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, CalculateStandardPoint);
                frm.ShowDialog();
            }
            else
            {
                MultipleStepsProcess(calibConfig, bind);

                CalculateStandardPoint(calibConfig, bind, position);
            }

            return new ProcessResponse(true);
        }

        public void SendCalibrationStartSignal(int stepNum)
        public void CalculateMatrix(CalibrationConfigCollection calibConfig, AGVBindUnit bind, PathPosition position)
        {
            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)
            {
                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);
        }

        private int MultipleStepsCalibration(CalibrationConfigCollection configs, Action<List<CalibrationConfig>> FinalCalculation)
        /// <summary>
        /// 获取标定点的转换矩阵
        /// </summary>
        /// <param name="points"></param>
        /// <returns></returns>
        protected List<double> GetMovementMatrix(List<CustomizedPoint> imagePoints, List<CustomizedPoint> platPoints, out string msg)
        {
            throw new NotImplementedException();
            HTuple uList, vList, xList, yList, matrix;

            //configs.Configs.ForEach(c =>
            //{
            //    c.Image = null;
            //});
            ConvertPointToHTuple(imagePoints, out uList, out vList);
            ConvertPointToHTuple(platPoints, out xList, out yList);

            //if (string.IsNullOrWhiteSpace(configs.CameraId) || !CameraDict.ContainsKey(configs.CameraId))
            //{
            //    throw new ProcessException("标定配置未配置正确的相机编号");
            //}
            HOperatorSet.VectorToHomMat2d(uList, vList, xList, yList, out matrix);

            //if (string.IsNullOrWhiteSpace(configs.PositionCode))
            //{
            //    throw new ProcessException("标定配置未指定路径位置");
            //}
            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);

            //CameraBase camera = CameraDict[configs.CameraId];
            //FrmCalib9PDynamic frm9PDynamic = new FrmCalib9PDynamic(this, camera, configs, FinalCalculation);
            //frm9PDynamic.ShowDialog();
                sum += Math.Sqrt((Math.Pow((m.D - platPoints[i].X), 2) + Math.Pow((n.D - platPoints[i].Y), 2)));
            }

            //if (configs.InputPara == null || configs.InputPara.Count <= 0)
            //{
            //    return (int)PLCReplyValue.NG;
            //}
            sum = sum / (double)imagePoints.Count;

            //if (configs.InputPara[0] <= 0 || configs.InputPara[0] > configs.Configs.Count)
            //{
            //    configs.InputPara = null;
            //    return (int)PLCReplyValue.IGNORE;
            //}
            msg = $"标定点数量:{imagePoints.Count};单点误差:{sum.ToString()}脉冲";

            //int sequence = configs.InputPara[0];
            return matrix.DArr.ToList();
        }

            ////第一次
            //if (sequence == 1)
            //{
            //    configs.Configs.ForEach(c =>
            //    {
            //        c.Image = null;
            //        c.OfflineImagePath = "";
            //        c.CurrentPlatPoint = new CustomizedPoint();
            //        c.ImageMarkPoint = new CustomizedPoint();
            //    });
            //}
        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());
        }

            //CalibrationConfig stepConfig = configs.Configs[sequence - 1];
        public void CalculateStandardPoint(CalibrationConfigCollection calibConfig, AGVBindUnit bind, PathPosition position)
        {
            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),
                });
            }

            //HDevEngineTool tool = _halconToolDict[]
            PubSubCenter.Publish(PubTag.CalibAllDone.ToString(), "", $"标定完成，标准点:{markPoint.GetDisplayText()}", true);
        }

            //CalibMarkPoint(camera, stepConfig, sequence);
        public void MultipleStepsProcess(CalibrationConfigCollection calibConfig, AGVBindUnit bind)
        {
            for (int i = 0; i < calibConfig.Configs.Count; i++)
            {
                SingleStepProcess(calibConfig.Configs[i], bind, i);
            }
        }

            ////获取当前平台点位
            //stepConfig.CurrentPlatPoint = new CustomizedPoint(_monitorList.Skip(locationStartIndex).Take(4).ToList());
            ////stepConfig.CurrentPlatPoint = new CustomizedPoint(configs.InputPara.Skip(1).Take(4).ToList());
        public void SingleStepProcess(CalibrationConfig config, AGVBindUnit bind, int index)
        {
            bind.Robot.Move(config.PlatPoint, MoveType.AbsoluteMove, true);

            ////最后一次
            //if (sequence == configs.Configs.Count)
            //{
            //    FinalCalculation?.Invoke(configs.Configs);
            //}
            using (HObject hImage = CollectHImage(bind.Camera, config.CameraOpConfig, "RobotCalibration"))
            {
                var tool = _halconToolDict[config.CameraOpConfig.AlgorithemPath];

            //configs.InputPara = null;
            //return (int)PLCReplyValue.OK;
                tool.InputImageDic.Clear();
                tool.InputImageDic["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);
        }
    }
}