Display_D_HP/TeamAAS-VM/ViewModels/Calibration/FixedUpCameraFindP0ViewModel.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MathNet.Numerics.LinearAlgebra;
using Prism.Commands;
using Prism.Events;
using Prism.Ioc;
using Prism.Mvvm;
using Prism.Regions;
using Prism.Services.Dialogs;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Shapes;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Core;
using TeamAAS_VP.Enums;
using TeamAAS_VP.Events;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP.Models.Calibration;
using TeamAAS_VP.Resources.Languages;
using TeamAAS_VP.Services;

namespace TeamAAS_VP.ViewModels.Calibration
{
    public class FixedUpCameraFindP0ViewModel : BindableBase, IConfirmNavigationRequest
    {
        IRegionManager _regionManager;
        IRegionNavigationService _regionNavigationService;
        IContainerProvider _container;
        IDialogService _dialogService;
        IEventAggregator _eventAggregator;
        IConfigService _configService;
        IRobotService _robotService;
        ICameraService _cameraService;
        IFeederService _feederService;
        ISystemDatabaseService _systemDatabaseService;
        ICalibrationService _calibrationServvice;

        #region 属性

        private CogToolBlockEditV2 _ToolBlockEdit;

        public CogToolBlockEditV2 ToolBlockEdit
        {
            get { return _ToolBlockEdit; }
            set { SetProperty(ref _ToolBlockEdit, value); }
        }


        private CalibrationInfo _SelectCalibration;

        public CalibrationInfo SelectCalibration
        {
            get { return _SelectCalibration; }
            set { SetProperty(ref _SelectCalibration, value); }
        }

        private IRobot _Robot;

        public IRobot Robot
        {
            get { return _Robot; }
            set { SetProperty(ref _Robot, value); }
        }

        private ICamera _Camera;

        public ICamera Camera
        {
            get { return _Camera; }
            set { SetProperty(ref _Camera, value); }
        }

        private float _ExposureTime;
        /// <summary>
        /// 相机曝光时间
        /// </summary>
        public float ExposureTime
        {
            get { return _ExposureTime; }
            set
            {
                SetProperty(ref _ExposureTime, value);
                if (Camera != null)
                {
                    try
                    {
                        SelectCalibration.FixedUpCameraFindP0Info.ExposureTime = value;
                        Camera.SetExposureTime(value);
                    }
                    catch (Exception)
                    {

                    }
                }
            }
        }

        private float _Gain;
        /// <summary>
        /// 相机增益
        /// </summary>
        public float Gain
        {
            get { return _Gain; }
            set
            {
                SetProperty(ref _Gain, value);
                if (Camera != null)
                {
                    try
                    {
                        SelectCalibration.FixedUpCameraFindP0Info.Gain = value;
                        Camera.SetGain(value);
                    }
                    catch (Exception)
                    {

                    }
                }
            }
        }

        private ObservableCollection<CameraInfo> _Cameras;
        public ObservableCollection<CameraInfo> Cameras
        {
            get { return _Cameras; }
            set { SetProperty(ref _Cameras, value); }
        }

        private CameraInfo _SelectCamera;
        public CameraInfo SelectCamera
        {
            get { return _SelectCamera; }
            set
            {
                SetProperty(ref _SelectCamera, value);
                if (value != null)
                {
                    Camera = _cameraService.GetCamera(value.Id);
                    SelectCalibration.FixedUpCameraFindP0Info.CameraId = value.Id;
                    SelectCalibration.FixedUpCameraFindP0Info.CameraName = value.CameraName;
                }
            }
        }

        private ObservableCollection<CalibrationInfo> _CalibrationList;
        public ObservableCollection<CalibrationInfo> CalibrationList
        {
            get { return _CalibrationList; }
            set { SetProperty(ref _CalibrationList, value); }
        }

        private CalibrationInfo _SelectedCalibration;
        public CalibrationInfo SelectedCalibration
        {
            get { return _SelectedCalibration; }
            set
            {
                SetProperty(ref _SelectedCalibration, value);
                if (value != null)
                {
                    SelectCalibration.FixedUpCameraFindP0Info.CalibrationId = value.Id;
                    SelectCalibration.FixedUpCameraFindP0Info.CalibrationName = value.Name;
                }
            }
        }

        #endregion

        #region 命令
        private DelegateCommand _LoadedCommand;
        public DelegateCommand LoadedCommand =>
            _LoadedCommand ?? (_LoadedCommand = new DelegateCommand(ExecuteLoadedCommand));
        private DelegateCommand _NextCommand;
        public DelegateCommand NextCommand =>
            _NextCommand ?? (_NextCommand = new DelegateCommand(ExecuteNextCommand));

        private DelegateCommand _BackCommand;
        public DelegateCommand BackCommand =>
            _BackCommand ?? (_BackCommand = new DelegateCommand(ExecuteBackCommand));

        private DelegateCommand _RunVisionProcessCommand;
        public DelegateCommand RunVisionProcessCommand =>
            _RunVisionProcessCommand ?? (_RunVisionProcessCommand = new DelegateCommand(ExecuteRunVisionProcessCommand));

        private DelegateCommand _FindP0Command;
        public DelegateCommand FindP0Command =>
            _FindP0Command ?? (_FindP0Command = new DelegateCommand(ExecuteFindP0Command));

        private DelegateCommand _TeachPutPosCommand;
        public DelegateCommand TeachPutPosCommand =>
            _TeachPutPosCommand ?? (_TeachPutPosCommand = new DelegateCommand(ExecuteTeachPutPosCommand));

        private DelegateCommand _MovePutPosCommand;
        public DelegateCommand MovePutPosCommand =>
            _MovePutPosCommand ?? (_MovePutPosCommand = new DelegateCommand(ExecuteMovePutPosCommand));

        private DelegateCommand _TeachDownCameraPosCommand;
        public DelegateCommand TeachDownCameraPosCommand =>
            _TeachDownCameraPosCommand ?? (_TeachDownCameraPosCommand = new DelegateCommand(ExecuteTeachDownCameraPosCommand));

        private DelegateCommand _MoveDownCameraPosCommand;
        public DelegateCommand MoveDownCameraPosCommand =>
            _MoveDownCameraPosCommand ?? (_MoveDownCameraPosCommand = new DelegateCommand(ExecuteMoveDownCameraPosCommand));

        private DelegateCommand _StartCalibP0Command;
        public DelegateCommand StartCalibP0Command =>
            _StartCalibP0Command ?? (_StartCalibP0Command = new DelegateCommand(ExecuteStartCalibP0Command));

        #endregion

        #region 事件

        #endregion

        public FixedUpCameraFindP0ViewModel(IRegionManager regionManager, IEventAggregator ea, IRegionNavigationService regionNavigationService, IContainerProvider container, IDialogService dialogService,
            IConfigService configService, IRobotService robotService, ICameraService cameraService, IFeederService feederService, ISystemDatabaseService systemDatabaseService, ICalibrationService calibrationService)
        {
            _regionManager = regionManager;
            _regionNavigationService = regionNavigationService;
            _container = container;
            _dialogService = dialogService;
            _eventAggregator = ea;
            _configService = configService;
            _robotService = robotService;
            _cameraService = cameraService;
            _feederService = feederService;
            _systemDatabaseService = systemDatabaseService;
            _calibrationServvice = calibrationService;
        }

        #region 方法
        void ExecuteLoadedCommand()
        {
            try
            {
                if (_systemDatabaseService.GetCurrentUser().userPart == Enums.UserPart.Operator)
                {
                    IsAllowEdit = false;
                }
                else
                {
                    IsAllowEdit = true;
                }
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准加载视觉工具处理界面时出错!", ex);
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 下一步
        /// </summary>
        void ExecuteNextCommand()
        {
            try
            {
                string prcPath = FilePath.CalibrationPath + "//" + SelectCalibration.Name + "//" + "FixedUpCameraTool.vpp";
                if (!Directory.Exists(System.IO.Path.GetDirectoryName(prcPath)))
                {
                    Directory.CreateDirectory(System.IO.Path.GetDirectoryName(prcPath));
                }
                VisionProFileHelper.SaveObjectToFileSafe(ToolBlockEdit.Subject, prcPath);
                NavigationParameters param = new NavigationParameters();
                param.Add("SelectCalibration", SelectCalibration);
                _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationAuto", param);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准-下相机示教P0界面点击下一步时出错!", ex);
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 上一步
        /// </summary>
        void ExecuteBackCommand()
        {
            try
            {
                NavigationParameters param = new NavigationParameters();
                param.Add("SelectCalibration", SelectCalibration);
                _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationTeachCenterPoint", param);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准-下相机示教P0界面点击上一步时出错!", ex);
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 运行视觉流程
        /// </summary>
        void ExecuteRunVisionProcessCommand()
        {
            try
            {
                if (Camera == null)
                {
                    MessageBox.Show(Lang.请先选择相机);
                    return;
                }
                ToolBlockEdit.Subject.Inputs["InputImage"].Value = Camera.Grab();
                ToolBlockEdit.Subject.Run();
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准-运行流程时出错时出错", ex);
                _eventAggregator.GetEvent<SnackbarMessageNotification>().Publish(new MessageParameter() { Msg = ex.Message, Duration = 1 });
            }
        }

        /// <summary>
        /// 自动计算P0点
        /// </summary>
        async void ExecuteFindP0Command()
        {
            if (Camera == null)
            {
                MessageBox.Show(Lang.请先选择相机);
                return;
            }
            if (SelectedCalibration == null)
            {
                MessageBox.Show(Lang.请先选择标定);
                return;
            }
            var result = await CaptureAndProcess(null);
            if (result.IsSuccess)
            {
                if (SelectCalibration.MarkPoint == null)
                {
                    SelectCalibration.MarkPoint = new RPoint();
                }
                SelectCalibration.MarkPoint.X = (float)result.X;
                SelectCalibration.MarkPoint.Y = (float)result.Y;
                MessageBox.Show($"P0点计算成功!X:{result.X},Y:{result.Y}");
            }
            else
            {
                MessageBox.Show("未找到P0点,请调整工件位置后重试!");
            }
        }

        /// <summary>
        /// 通过下相机开始标定P0点
        /// </summary>
        async void ExecuteStartCalibP0Command()
        {
            //弹窗确认是否开始标定
            var result = MessageBox.Show(Lang.确认通过下相机开始标定P0点, Lang.提示, MessageBoxButton.YesNo, MessageBoxImage.Question);
            if (result != MessageBoxResult.Yes)
            {
                return;
            }

            try
            {
                // 1.移动至标定块放置位置，取标定块

                //移动机器人至下相机位置
                bool isSuccess = await Robot.CalibMotionAsync(SelectCalibration.PutPoint, null);
                if (!isSuccess)
                {
                    MessageBox.Show(Lang.移动至移动至标定块放置位置取标定块失败);
                    return;
                }
                //打开吸气
                Robot.CalibOutIO(true);
                await Task.Delay(300);

                // 2.移动至下相机位置，拍照，计算P0点
                isSuccess = await Robot.CalibMotionAsync(SelectCalibration.DownCameraPoint, null);
                if (!isSuccess)
                {
                    MessageBox.Show(Lang.移动至下相机位置失败);
                    return;
                }

                var calibResult = await CaptureAndProcess(new double[] { SelectCalibration.DownCameraPoint.X, SelectCalibration.DownCameraPoint.Y, SelectCalibration.DownCameraPoint.U });
                if (calibResult.IsSuccess)
                {
                    //工具坐标
                    double toolX = calibResult.X;
                    double toolY = calibResult.Y;

                    //创建工具坐标
                    ToolCoord toolCoord = new ToolCoord();
                    toolCoord.SetTool(toolX, toolY);

                    Vector<double> P10 = Vector<double>.Build.Dense(new double[] { SelectCalibration.PutPoint.X, SelectCalibration.PutPoint.Y });
                    var p0 = toolCoord.GetToolnCoord(P10, SelectCalibration.PutPoint.U);

                    //计算P0点
                    if (SelectCalibration.MarkPoint == null)
                    {
                        SelectCalibration.MarkPoint = new RPoint();
                    }
                    SelectCalibration.MarkPoint.X = (float)p0[0];
                    SelectCalibration.MarkPoint.Y = (float)p0[1];
                    //MessageBox.Show($"P0点计算成功!X:{calibResult.X},Y:{calibResult.Y}");
                }
                else
                {
                    MessageBox.Show(Lang.未找到P0点请调整工件位置后重试);
                    return;
                }

                // 3.移动至标定块放置位置，放下标定块
                isSuccess = await Robot.CalibMotionAsync(SelectCalibration.PutPoint, null);
                if (isSuccess)
                {
                    //关闭吸气
                    Robot.CalibOutIO(false);
                    await Task.Delay(300);

                }
                else
                {
                    MessageBox.Show(Lang.移动至标定块放置位置放下标定块失败);
                    return;
                }

                // 4.Z轴上升安全高度
                RPoint safePoint = SelectCalibration.PutPoint.Clone();
                safePoint.Z = 0;
                isSuccess = await Robot.CalibMotionAsync(safePoint, null);
                if (!isSuccess)
                {
                    MessageBox.Show(Lang.Z轴上升至安全高度失败);
                    return;
                }

                //完成提示
                MessageBox.Show(Lang.通过下相机标定P0点完成);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准-通过下相机开始标定P0点时出错!", ex);
                MessageBox.Show(ex.Message);
            }


        }

        /// <summary>
        /// 移动至下相机位置
        /// </summary>
        async void ExecuteMoveDownCameraPosCommand()
        {
            try
            {
                //弹窗确认是否移动
                var result = MessageBox.Show(Lang.确认移动至下相机位置, Lang.提示, MessageBoxButton.YesNo, MessageBoxImage.Question);
                if (result != MessageBoxResult.Yes)
                {
                    return;
                }
                //移动机器人至下相机位置
                bool isSuccess = await Robot.CalibMotionAsync(SelectCalibration.DownCameraPoint, null);
                if (isSuccess)
                {
                    MessageBox.Show(Lang.移动至下相机位置成功);
                }
                else
                {
                    MessageBox.Show(Lang.移动至下相机位置失败);
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 示教下相机位置
        /// </summary>
        void ExecuteTeachDownCameraPosCommand()
        {
            try
            {
                //弹窗确认是否示教
                var result = MessageBox.Show(Lang.确认示教下相机位置, Lang.提示, MessageBoxButton.YesNo, MessageBoxImage.Question);
                if (result != MessageBoxResult.Yes)
                {
                    return;
                }

                //获取机器人当前位置
                SelectCalibration.DownCameraPoint = Robot.GetRobotPos();
                MessageBox.Show(Lang.示教下相机位置成功);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 移动至标定块放位置
        /// </summary>
        async void ExecuteMovePutPosCommand()
        {
            try
            {
                //弹窗确认是否移动
                var result = MessageBox.Show(Lang.确认移动至标定块放位置, Lang.提示, MessageBoxButton.YesNo, MessageBoxImage.Question);
                if (result != MessageBoxResult.Yes)
                {
                    return;
                }
                //移动机器人至标定块放位置
                bool isSuccess = await Robot.CalibMotionAsync(SelectCalibration.PutPoint, null);
                if (isSuccess)
                {
                    MessageBox.Show(Lang.移动至标定块放位置成功);
                }
                else
                {
                    MessageBox.Show(Lang.移动至标定块放位置失败);
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 示教标定块放位置
        /// </summary>
        void ExecuteTeachPutPosCommand()
        {
            try
            {
                //弹窗确认是否示教
                var result = MessageBox.Show(Lang.确认示教标定块放位置, Lang.提示, MessageBoxButton.YesNo, MessageBoxImage.Question);
                if (result != MessageBoxResult.Yes)
                {
                    return;
                }

                //获取机器人当前位置
                SelectCalibration.PutPoint = Robot.GetRobotPos();
                MessageBox.Show(Lang.示教标定块放位置成功);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        /// <summary>
        /// 相机拍照
        /// </summary>
        /// <returns></returns>
        private async Task<(bool IsSuccess, double X, double Y, double U, int ImageWidth, int ImageHeight)> CaptureAndProcess(double[] robotCoord)
        {

            ICogImage image = null;
            //采集图像
            if (Camera != null)
            {
                image = Camera.Grab();
                if (SelectedCalibration.IsDistortionCorrection)
                {
                    if (SelectedCalibration.CalibCheckerboardTool==null)
                    {
                        string prcPath = FilePath.CalibrationPath + "//" + SelectedCalibration.Name + "//" + "CalibCheckerboardTool.vpp";
                        if (File.Exists(prcPath))
                        {
                            SelectedCalibration.CalibCheckerboardTool = VisionProFileHelper.LoadObjectFromFileSafe<Cognex.VisionPro.CalibFix.CogCalibCheckerboardTool>(prcPath);
                            SelectedCalibration.CalibCheckerboardTool.InputImage = image;
                            SelectedCalibration.CalibCheckerboardTool.Run();
                            image = SelectedCalibration.CalibCheckerboardTool.OutputImage;
                        }
                    }
                }
                ToolBlockEdit.Subject.Inputs["InputImage"].Value = image;
            }
            ToolBlockEdit.Subject.Run();
            CogToolBlockTerminalCollection outputCollection = ToolBlockEdit.Subject.Outputs;
            bool Found = (bool)(outputCollection["Found"].Value);
            if (Found)
            {
                var strpoints = outputCollection["Point"].Value.ToString();
                string[] items = strpoints.Split(',');
                double x = double.Parse(items[0]);
                double y = double.Parse(items[1]);

                //校准转换
                var calibResult = _calibrationServvice.ConvertPixelToPosition((x, y, 0), robotCoord, SelectedCalibration, Robot.Brand);
                if (calibResult.IsSucceed)
                {
                    x = calibResult.X;
                    y = calibResult.Y;
                    return (true, x, y, 0, image.Width, image.Height);
                }

                return (false, 0, 0, 0, image.Width, image.Height);
            }
            else
            {
                return (false, 0, 0, 0, image.Width, image.Height);
            }
        }
        #endregion

        #region 继承
        /// <summary>
        /// 确认导航请求时调用。此方法允许您在导航之前执行一些操作。
        /// </summary>
        /// <param name="navigationContext"></param>
        /// <param name="continuationCallback"></param>
        public void ConfirmNavigationRequest(NavigationContext navigationContext, Action<bool> continuationCallback)
        {
            continuationCallback(true);
        }

        /// <summary>
        /// 接收导航请求时调用。传入导航参数，包含有关导航目标的信息。
        /// </summary>
        /// <param name="navigationContext"></param>
        /// <exception cref="NotImplementedException"></exception>
        public async void OnNavigatedTo(NavigationContext navigationContext)
        {
            SelectCalibration = navigationContext.Parameters.GetValue<CalibrationInfo>("SelectCalibration");
            if (SelectCalibration.FixedUpCameraFindP0Info == null)
            {
                SelectCalibration.FixedUpCameraFindP0Info = new FixedUpCameraFindP0Info();
            }
            string prcPath = FilePath.CalibrationPath + "//" + SelectCalibration.Name + "//" + "FixedUpCameraTool.vpp";
            if (File.Exists(prcPath))
            {
                ToolBlockEdit.Subject = VisionProFileHelper.LoadObjectFromFileSafe<CogToolBlock>(prcPath);
            }
            else
            {
                ToolBlockEdit.Subject = VisionProFileHelper.LoadObjectFromFileSafe<CogToolBlock>(FilePath.CalibrationToolblockPath);
            }

            if (SelectCalibration.RobotId != Guid.Empty)
            {
                Robot = _robotService.GetRobot(SelectCalibration.RobotId);
            }

            Cameras = new ObservableCollection<CameraInfo>(_configService.GetAllCameras());
            if (SelectCalibration.FixedUpCameraFindP0Info.CameraId != Guid.Empty)
            {
                SelectCamera = Cameras.FirstOrDefault(c => c.Id == SelectCalibration.FixedUpCameraFindP0Info.CameraId);

            }
            CalibrationList = new ObservableCollection<CalibrationInfo>(_calibrationServvice.GetAllCalibrations());
            if (SelectCalibration.FixedUpCameraFindP0Info.CalibrationId != Guid.Empty)
            {
                SelectedCalibration = CalibrationList.FirstOrDefault(c => c.Id == SelectCalibration.FixedUpCameraFindP0Info.CalibrationId);
            }

            ExposureTime = SelectCalibration.FixedUpCameraFindP0Info.ExposureTime==0?5000: SelectCalibration.FixedUpCameraFindP0Info.ExposureTime;
            Gain = SelectCalibration.FixedUpCameraFindP0Info.Gain;

        }

        /// <summary>
        /// 是否允许导航到此视图模型。
        /// </summary>
        /// <param name="navigationContext"></param>
        /// <returns></returns>
        public bool IsNavigationTarget(NavigationContext navigationContext)
        {
            return true;
        }

        /// <summary>
        /// 导航离开此视图模型时调用。您可以在此处执行清理操作或保存状态。
        /// </summary>
        /// <param name="navigationContext"></param>
        public void OnNavigatedFrom(NavigationContext navigationContext)
        {

        }
        #endregion

        private bool _IsAllowEdit = false;

        public bool IsAllowEdit
        {
            get { return _IsAllowEdit; }
            set { SetProperty(ref _IsAllowEdit, value); }
        }

        private void LoginChange(Models.User user)
        {
            if (user.userPart == Enums.UserPart.Operator)
            {
                IsAllowEdit = false;
            }
            else
            {
                IsAllowEdit = true;
            }
        }
    }
}