派生自 A032
blame | 历史 | 补丁 | 提交 | 提交对比 | ignore whitespace
patrick
2019-10-22 7971d855ba2cf9772b46a7f67e2b669c0afcb91b 
 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();


    }


}