Display_D_HP/TeamAAS-VM/Services/CalibrationService.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using System.Windows.Forms;
using TeamAAS_VP.Core;
using TeamAAS_VP.Enums;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models.Calibration;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.Services
{
    /// <summary>
    /// Calibration 管理服务，负责校准文件的读取与保存
    /// </summary>
    public class CalibrationService : ICalibrationService
    {
        private static class Paths
        {
            public static readonly string CalibrationPath = "..//Calibration";
            public static string CalibrationListFilePath => Path.Combine(CalibrationPath ?? string.Empty, "CalibrationList.cfg");
        }

        private readonly object _sync = new object();
        private ObservableCollection<CalibrationInfo> _calibrations = new ObservableCollection<CalibrationInfo>();

        public CalibrationService()
        {
            // 不在构造中自动加载，调用者可以选择 LoadAll
        }

        public IReadOnlyCollection<CalibrationInfo> GetAllCalibrations()
        {
            lock (_sync)
            {
                return _calibrations.ToList().AsReadOnly();
            }
        }

        public CalibrationInfo GetCalibration(Guid id)
        {
            lock (_sync)
            {
                return _calibrations.FirstOrDefault(c => c.Id == id);
            }
        }

        public CalibrationInfo AddOrUpdateCalibration(CalibrationInfo calib)
        {
            if (calib == null) return null;
            lock (_sync)
            {
                var exist = _calibrations.FirstOrDefault(c => c.Id == calib.Id);
                if (exist != null)
                {
                    var idx = _calibrations.IndexOf(exist);
                    _calibrations[idx] = calib;
                }
                else
                {
                    calib.Index = _calibrations.Count + 1;
                    _calibrations.Add(calib);
                }
            }

            calib.DateTime = DateTime.Now;
            string folder = Path.Combine(Paths.CalibrationPath, calib.Name);
            string calibFile = Path.Combine(folder, calib.Name + ".cfg");

            if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);

            // 写入单个校准文件
            FileHelper.WriteJsonFile(calib, calibFile);

            // 更新索引列表
            var list = new Dictionary<Guid, string>();
            lock (_sync)
            {
                foreach (var item in _calibrations)
                {
                    list[item.Id] = item.Name;
                }
            }
            FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);

            // 保存ToolBlock
            string prcPath = Path.Combine(folder, calib.Name + ".vpp");
            if (calib.ToolBlock != null)
            {
                CogSerializer.SaveObjectToFile(calib.ToolBlock, prcPath);
            }
            else
            {
                if (File.Exists(prcPath))
                {
                    calib.ToolBlock = CogSerializer.LoadObjectFromFile(prcPath) as CogToolBlock;
                }
                else if (File.Exists("..//Vision Template//Calibration.vpp"))
                {
                    calib.ToolBlock = CogSerializer.LoadObjectFromFile("..//Vision Template//Calibration.vpp") as CogToolBlock;
                    CogSerializer.SaveObjectToFile(calib.ToolBlock, prcPath);
                }
            }

            return calib;
        }

        public bool RemoveCalibration(Guid id)
        {
            bool result = false;
            CalibrationInfo removed = null;
            lock (_sync)
            {
                removed = _calibrations.FirstOrDefault(c => c.Id == id);
                if (removed != null) result = _calibrations.Remove(removed);
            }

            // 更新列表文件
            var list = new Dictionary<Guid, string>();
            lock (_sync)
            {
                foreach (var item in _calibrations)
                {
                    list[item.Id] = item.Name;
                }
            }
            FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);

            //删除后，对所有校准重新排序索引
            //先按照Index进行排序
            var calibs = _calibrations.OrderBy(c => c.Index).ToList();
            int index= 1;
            foreach (var item in calibs)
            {
                item.Index = index;
                //保存更新后的索引
                string folder = Path.Combine(Paths.CalibrationPath, item.Name);
                string calibFile = Path.Combine(folder, item.Name + ".cfg");
                FileHelper.WriteJsonFile(item, calibFile);
                index++;
            }
            _calibrations = new ObservableCollection<CalibrationInfo>(calibs);

            // 可选：删除物理文件和toolblock目录（不自动删除，除非明确需求）

            return result;
        }

        public void LoadAll()
        {
            lock (_sync)
            {
                if (!Directory.Exists(Paths.CalibrationPath)) Directory.CreateDirectory(Paths.CalibrationPath);

                _calibrations.Clear();
                if (!File.Exists(Paths.CalibrationListFilePath))
                {
                    FileHelper.WriteJsonFile(new Dictionary<Guid, string>(), Paths.CalibrationListFilePath);
                    return;
                }

                var list = FileHelper.ReadJsonFile<Dictionary<Guid, string>>(Paths.CalibrationListFilePath);
                if (list == null) return;

                foreach (var item in list)
                {
                    try
                    {
                        string filePath = Path.Combine(FilePath.CalibrationPath, item.Value, item.Value + ".cfg");
                        if (File.Exists(filePath))
                        {
                            var calib = FileHelper.ReadJsonFile<CalibrationInfo>(filePath);
                            // 尝试加载toolblock
                            string prcPath = Path.Combine(FilePath.CalibrationPath, calib.Name, calib.Name + ".vpp");
                            if (File.Exists(prcPath))
                            {
                                calib.ToolBlock = CogSerializer.LoadObjectFromFile(prcPath) as CogToolBlock;
                            }
                            _calibrations.Add(calib);
                        }
                    }
                    catch
                    {
                        // 忽略单个文件错误，继续加载其他文件
                    }
                }
            }
        }

        public void SaveAll()
        {
            lock (_sync)
            {
                var list = new Dictionary<Guid, string>();
                foreach (var item in _calibrations)
                {
                    list[item.Id] = item.Name;
                    string folder = Path.Combine(FilePath.CalibrationPath, item.Name);
                    if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);
                    string filePath = Path.Combine(folder, item.Name + ".cfg");
                    FileHelper.WriteJsonFile(item, filePath);
                    if (item.ToolBlock != null)
                    {
                        string prcPath = Path.Combine(folder, item.Name + ".vpp");
                        CogSerializer.SaveObjectToFile(item.ToolBlock, prcPath);
                    }
                }
                FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);
            }
        }

        /// <summary>
        /// 校准转换，将像素坐标转换位置坐标
        /// </summary>
        /// <param name="pixelCoord"></param>
        /// <param name="robotCoord"></param>
        /// <param name="calib"></param>
        /// <returns></returns>
        public (bool IsSucceed,double X, double Y, double U) ConvertPixelToPosition((double X, double Y, double angle) pixelCoord, double[] robotCoord, CalibrationInfo calib, RobotBrand robotBrand = RobotBrand.Default)
        {
            double angle= pixelCoord.angle;
            //得到校准矩阵
            Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
            if (calib.CameraMount == CameraMount.FixedDown || calib.CameraMount == CameraMount.MobileJ2 || calib.CameraMount == CameraMount.MobileJ4)
            {
                angle *= -1;
            }
            //将像素坐标转换成机器人坐标
            var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelCoord.X, pixelCoord.Y, 1 });
            if (calib.CameraMount == Enums.CameraMount.MobileJ4)
            {
                //像素直接转换成mm时的点位
                double Robot_X, Robot_Y;
                Robot_X = rpos[0];
                Robot_Y = rpos[1];

                //机器人当前TOOL 0下的坐标
                double curpos_x = 0, curpos_y = 0, curpos_u = 0;
                curpos_x = robotCoord[0];
                curpos_y = robotCoord[1];
                curpos_u = robotCoord[2];

                //得到旋转矩阵
                double angle1 = Math.PI * (curpos_u - calib.MarkPoint.U) / 180;
                // 创建T矩阵
                Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
                {
                                            { Math.Cos(angle1), -Math.Sin(angle1) },
                                            { Math.Sin(angle1), Math.Cos(angle1) }
                });

                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calib.CalibPoints[0].Robot.X, Robot_Y - calib.CalibPoints[0].Robot.Y });
                Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });

                var cc = rotationMatrix * p3 + phere;
                rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] });
            }
            else if (calib.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
            {
                //像素直接转换成mm时的点位
                double Robot_X, Robot_Y;
                Robot_X = rpos[0];
                Robot_Y = rpos[1];

                //模组当前TOOL 0下的坐标（相当于模组吸嘴坐标）
                double curpos_x = 0, curpos_y = 0, curpos_u = 0;
                curpos_x = robotCoord[0];
                curpos_y = robotCoord[1];
                curpos_u = robotCoord[2];

                //得到旋转矩阵
                //double angle1 = Math.PI * (curpos_u - calib.MarkPoint.U) / 180;
                double angle1 = 0;
                // 创建T矩阵
                Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
                {
                                            { Math.Cos(angle1), -Math.Sin(angle1) },
                                            { Math.Sin(angle1), Math.Cos(angle1) }
                });

                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calib.CalibPoints[0].Robot.X, Robot_Y - calib.CalibPoints[0].Robot.Y });
                Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });

                var cc = rotationMatrix * p3 + phere;
                rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] });//Mark点在tool0下的的坐标
            }
            else
            {
                //当需要直接转换工具坐标时
                if (robotCoord!=null)
                {
                    double curposX = robotCoord[0];
                    double curposY = robotCoord[1];
                    double curposU = robotCoord[2];

                    //根据机器人当前的点位计算工具坐标
                    ToolCoord tool = new ToolCoord();
                    if (robotBrand == RobotBrand.Schneider)
                    {
                        curposU *= -1;
                    }
                    tool.ComputeTool(curposX, curposY, curposU, rpos[0], rpos[1]);
                    rpos = Vector<double>.Build.Dense(new double[] { tool.X, tool.Y });
                }
            }
            return (true, rpos[0], rpos[1], angle);
        }

        /// <summary>
        /// 校准转换，将像素坐标转换位置坐标
        /// </summary>
        /// <param name="pixelCoord"></param>
        /// <param name="robotCoord"></param>
        /// <param name="calibId"></param>
        /// <returns></returns>
        public (bool IsSucceed, double X, double Y, double U) ConvertPixelToPosition((double X, double Y, double angle) pixelCoord, double[] robotCoord, Guid calibId, RobotBrand robotBrand = RobotBrand.Default)
        {

            var calib = GetCalibration(calibId);
            if (calib == null)
            {
                return (false, 0, 0, 0);
            }
            var result = ConvertPixelToPosition(pixelCoord, robotCoord, calib, robotBrand);
            return result;

        }

        public void Dispose()
        {
            SaveAll();
            GC.SuppressFinalize(this);
        }
    }
}