1. 重构项目

patrick

2019-12-10 1c4426810c71eead57084be8a18ade8d314dd8c4

 src/A032.Process/ProcessControl_Method.cs

@@ -1,259 +1,13 @@
using Bro.Common.Base;
using Bro.Common.Helper;
using Bro.Common.Helper;
using Bro.Common.Interface;
using Bro.Common.Model;
using Bro.Device.AuboRobot;
using Bro.Device.HikCamera;
using Bro.Device.SeerAGV;
using HalconDotNet;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace A032.Process
{
    public partial class ProcessControl
    {
        Dictionary<int, int> machineFullTrayDict = new Dictionary<int, int>();
        Dictionary<int, int> machineEmptyTrayDict = new Dictionary<int, int>();
        List<TaskAssignInfo> taskAssignedList = new List<TaskAssignInfo>();

        private void OnAGVTaskStatusChanged(SeerAGVDriver agv, AGVTaskStatus taskStatus)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.AGVId == agv.Id);

            if (bind == null)
            {
                throw new ProcessException("未能根据AGV信息获取绑定设备信息", null);
            }

            if (bind.AGVDest == agv.CurrentPosition && taskStatus == AGVTaskStatus.Completed)
            {
                //PathPosition loadEmptyTrayPosition = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

                //if (bind.AGVDest == loadEmptyTrayPosition.PositionCode)
                //{
                //    bind.RobotStatus = TaskStatus.Running;
                //}

                bind.AGVStatus = TaskStatus.Available;
            }
        }

        private void OnAGVPositionChanged(SeerAGVDriver agv, string positionCode)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.AGVId == agv.Id);

            if (bind == null)
            {
                throw new ProcessException("未能根据AGV信息获取绑定设备信息", null);
            }

            if (bind.AGVDest == positionCode && agv.TaskStatus == AGVTaskStatus.Completed)
            {
                //PathPosition loadEmptyTrayPosition = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

                //if (bind.AGVDest == loadEmptyTrayPosition.PositionCode)
                //{
                //    bind.RobotStatus = TaskStatus.Running;
                //}

                bind.AGVStatus = TaskStatus.Available;
            }
        }

        private void OnRobotMsgReceived(DateTime dt, AuboRobotDriver robot, RobotMsg msg)
        {
            LogAsync(dt, robot.Name + "接收信息", msg.GetDisplayText());

            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robot.Id);

            if (bind == null)
            {
                throw new ProcessException("未能根据机器人信息获取绑定设备信息", null);
            }

            List<AGVTaskModel> models = new List<AGVTaskModel>();

            switch (msg.Action)
            {
                case RobotMsgAction.Load:
                    {
                        switch (msg.Para1)
                        {
                            case RobotMsgParas.EmptyTray:
                                {
                                    bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;
                                }
                                break;
                            case RobotMsgParas.FullTray:
                                {
                                    machineFullTrayDict[msg.Para2]--;
                                    bind.CurrentFullTray = int.Parse(msg.Datas[1]);

                                    if (machineFullTrayDict[msg.Para2] > 0 && !bind.IsFullTrayFull)
                                    {
                                        RobotMsg_UnloadEmptyTray.Para2 = msg.Para2;
                                        robot.SendMsg(RobotMsg_UnloadEmptyTray, true);
                                    }
                                    else
                                    {
                                        bind.RobotStatus = TaskStatus.Available;
                                    }
                                }
                                break;
                            default:
                                break;
                        }
                    }
                    break;
                case RobotMsgAction.Unload:
                    {
                        switch (msg.Para1)
                        {
                            case RobotMsgParas.EmptyTray:
                                {
                                    machineEmptyTrayDict[msg.Para2]++;
                                    bind.CurrentEmptyTray = int.Parse(msg.Datas[0]);

                                    if (machineEmptyTrayDict[msg.Para2] < Config.Machine_EmptyTrayNum)
                                    {
                                        bind.RobotIOHandle.Reset();
                                        bind.RobotIOHandle.WaitOne();

                                        if (!bind.IsFullTrayFull)
                                        {
                                            RobotMsg_UnloadEmptyTray.Para2 = msg.Para2;
                                            robot.SendMsg(RobotMsg_UnloadEmptyTray, true);
                                        }
                                    }
                                    else
                                    {
                                        bind.RobotStatus = TaskStatus.Available;
                                    }
                                }
                                break;
                            case RobotMsgParas.FullTray:
                                {
                                    bind.CurrentFullTray = int.Parse(msg.Datas[1]);

                                    bind.RobotIOHandle.Reset();
                                    bind.RobotIOHandle.WaitOne();

                                    if (!bind.IsFullTrayEmpty)
                                    {
                                        Camera_UnloadFullTray(robot.Id, msg.Para2);
                                    }
                                }
                                break;
                            default:
                                break;
                        }
                    }
                    break;
                case RobotMsgAction.Move:
                    {
                        switch (msg.Para1)
                        {
                            case RobotMsgParas.LineSnap:
                                {
                                    Camera_UnloadFullTray(robot.Id, msg.Para2);
                                }
                                break;
                            case RobotMsgParas.LoadFullTraySnap:
                                {
                                    Camera_LoadFullTray(robot.Id, msg.Para2);
                                }
                                break;
                            case RobotMsgParas.UnloadEmptyTraySnap:
                                {
                                    Camera_UnloadEmptyTray(robot.Id, msg.Para2);
                                }
                                break;
                            case RobotMsgParas.Home:
                                {
                                    bind.RobotStatus = TaskStatus.Available;
                                }
                                break;
                            default:
                                break;
                        }
                    }
                    break;
                case RobotMsgAction.Calibration:
                    {

                    }
                    break;
                case RobotMsgAction.StandardPoint:
                    {

                    }
                    break;
                default:
                    break;
            }

            if (models.Count > 0)
            {
                models.ForEach(model =>
                {
                    if (!bind.TaskList.Any(t => t.MethodFunc.Method.Name == model.MethodFunc.Method.Name))
                    {
                        model.OpConfig = new AGVBindOpConfig(bind.Id);
                        bind.TaskList.Add(model);
                    }
                });
            }
        }

        public void QueryRobotIO()
        {
            RobotDict.Values.ToList().ForEach(r =>
            {
                r.SendMsg(RobotMsgAction.IO, RobotMsgParas.Query, 0);
            });
        }

        private void OnBindUnitTaskInvoke(AGVBindUnit bind)
        {
            var task = bind.TaskList[0];
            var response = task.MethodFunc.Invoke(task.OpConfig, task.Device);
        }

        #region Robot监听事件
        private void AddNewTaskToBind(string robotId, List<AGVTaskModel> models)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);

            if (bind == null)
            {
                throw new ProcessException("未能根据机器人信息获取绑定设备信息", null);
            }

            AddNewTaskToBind(bind, models);
        }

        private void AddNewTaskToBind(AGVBindUnit bind, List<AGVTaskModel> models)
        {
            if (models.Count > 0)
            {
                models.ForEach(model =>
                {
                    if (!bind.TaskList.Any(t => t.MethodFunc.Method.Name == model.MethodFunc.Method.Name))
                    {
                        model.OpConfig = new AGVBindOpConfig(bind.Id);
                        bind.TaskList.Add(model);
                    }
                });
            }
        }

        [ProcessMethod("", "Robot_Monitor_Alarm", "机器人监听事件-报警", true)]
        public ProcessResponse Robot_Monitor_Alarm(IOperationConfig config, IDevice device)
        {
@@ -261,11 +15,11 @@

            if (bind == null)
            {
                throw new ProcessException("未能根据机器人信息获取绑定设备信息", null);
                throw new ProcessException($"未能获取{device.Name}的绑定设备信息");
            }

            bind.AGV.PauseTask();
            bind.RobotStatus = TaskStatus.Warning;
            bind.AGV.CancelTask();
            bind.UnitState = AGVState.Warning;

            return new ProcessResponse(true);
        }
@@ -273,76 +27,58 @@
        [ProcessMethod("", "Robot_Monitor_EmptyTrayEmpty", "机器人监听事件-空Tray区域清空", true)]
        public ProcessResponse Robot_Monitor_EmptyTrayEmpty(IOperationConfig config, IDevice device)
        {
            bool isEmptyTrayEmpty = config.InputPara[0] == 1;
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
            if (isEmptyTrayEmpty)

            if (bind == null)
            {
                bind.IsEmptyTrayEmpty = true;

                Task.Run(() =>
                {
                    Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_LoadEmptyTray;
                    while (bind.IsEmptyTrayEmpty && bind.TaskList.Count == 0 && !bind.TaskList.Any(u => u.MethodFunc.Method.Name == action.Method.Name))
                    {
                        if (bind.TaskList.Count == 0)
                        {
                            List<AGVTaskModel> models = new List<AGVTaskModel>();
                            models.Add(new AGVTaskModel(TaskAvailableLevel.Both, AGV_LoadEmptyTray));
                            models.Add(new AGVTaskModel(TaskAvailableLevel.AGV, AfterEmptyTrayPositionArrived));

                            AddNewTaskToBind(device.Id, models);
                        }
                        else
                        {
                            Thread.Sleep(500);
                        }
                    }
                });

                throw new ProcessException($"未能获取{device.Name}的绑定设备信息");
            }
            else
            {
                bind.IsEmptyTrayEmpty = false;
            }

            bind.EmptyTrayNum = 0;

            TrayTask task = new TrayTask();
            task.TaskType = TaskType.LoadEmptyTrayToAGV;
            //task.Priority = 10;
            task.SourceDeviceId = device.Id;
            //task.SourceDeviceName = device.Name;

            //如果目前地址被占用，地址有可能为空，需要在任务指派时再次确认
            task.Location = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.LoadEmptyTray);

            InsertTask(task);

            return new ProcessResponse(true);
        }

        /// <summary>
        /// 信号要求长信号触发，避免误触发
        /// </summary>
        /// <param name="config"></param>
        /// <param name="device"></param>
        /// <returns></returns>
        [ProcessMethod("", "Robot_Monitor_FullTrayFull", "机器人监听事件-满Tray区域放满", true)]
        public ProcessResponse Robot_Monitor_FullTrayFull(IOperationConfig config, IDevice device)
        {
            bool isFullTrayFull = config.InputPara[0] == 1;
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
            if (isFullTrayFull)
            {
                bind.IsFullTrayFull = true;

                Task.Run(() =>
                {
                    Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_UnloadFullTray;
                    while (bind.IsFullTrayFull && !bind.TaskList.Any(u => u.MethodFunc.Method.Name == action.Method.Name))
                    {
                        if (bind.TaskList.Count == 0)
                        {
                            List<AGVTaskModel> models = new List<AGVTaskModel>();
                            models.Add(new AGVTaskModel(TaskAvailableLevel.Both, AGV_UnloadFullTray));
                            models.Add(new AGVTaskModel(TaskAvailableLevel.AGV, Robot_UnloadFullTray));

                            AddNewTaskToBind(device.Id, models);
                        }
                        else
                        {
                            Thread.Sleep(500);
                        }
                    }
                });
            }
            else
            if (bind == null)
            {
                bind.IsFullTrayFull = false;
                throw new ProcessException($"未能获取{device.Name}的绑定设备信息");
            }

            bind.RobotIOHandle.Set();
            //if (bind.FullTrayNum >= Config.AGVAvailableTrayNums)
            {
                bind.FullTrayNum = Config.AGVAvailableTrayNums;

                TrayTask task = new TrayTask();
                task.TaskType = TaskType.UnloadFullTrayToLine;
                task.SourceDeviceId = device.Id;

                //如果目前地址被占用，地址有可能为空，需要在任务指派时再次确认
                task.Location = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.UnloadFullTray);

                InsertTask(task);
            }

            return new ProcessResponse(true);
        }
@@ -350,11 +86,14 @@
        [ProcessMethod("", "Robot_Monitor_FullTrayEmpty", "机器人监听事件-满Tray区域清空", true)]
        public ProcessResponse Robot_Monitor_FullTrayEmpty(IOperationConfig config, IDevice device)
        {
            bool isFullTrayEmpty = config.InputPara[0] == 1;
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
            bind.IsFullTrayEmpty = isFullTrayEmpty;

            bind.RobotIOHandle.Set();
            if (bind == null)
            {
                throw new ProcessException($"未能获取{device.Name}的绑定设备信息");
            }

            bind.FullTrayNum = 0;

            return new ProcessResponse(true);
        }
@@ -390,191 +129,322 @@

            if (bind == null)
            {
                throw new ProcessException("未能根据机器人信息获取绑定设备信息", null);
                throw new ProcessException($"未能获取{device.Name}的绑定设备信息");
            }
            bind.AGV.CancelTask();

            //isEmptyTrayTaskAssigned = false;
            //isFullTrayTaskAssigned = false;

            taskAssignedList.RemoveAll(u => u.AgvId == device.Id);

            bind.TaskList.Clear();
            bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;
            Reset(bind.Id);

            return new ProcessResponse(true);
        }
        #endregion

        #region 空Tray上料
        [ProcessMethod("", "AGV_LoadEmptyTray", "AGV去往空Tray上料", true)]
        public ProcessResponse AGV_LoadEmptyTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置空Tray上料点");
            }

            bind.AGVDest = position.PositionCode;
            bind.AGV.TaskOrder(position.PositionCode);

            bind.AGVStatus = TaskStatus.Running;

            return new ProcessResponse(true);
        }

        [ProcessMethod("", "AfterEmptyTrayPositionArrived", "到达空Tray上料点", true)]
        public ProcessResponse AfterEmptyTrayPositionArrived(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置空Tray上料点", null);
            }

            if (bind.AGV.CurrentPosition != position.PositionCode)
            {
                throw new ProcessException("AGV尚未到达空Tray上料点", null);
            }

            bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.EmptyTray, 0);
            bind.RobotStatus = TaskStatus.Running;

            return new ProcessResponse(true);
        }

        //[ProcessMethod("", "EmptyTrayReady", "空Tray上料完成", true)]
        //public ProcessResponse EmptyTrayReady(IOperationConfig config, IDevice device)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        //    bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;

        //    return new ProcessResponse(true);
        //}
        #endregion

        #region 空Tray往机台下料
        //bool isEmptyTrayNeed = false;
        //bool isEmptyTrayTaskAssigned = false;
        RobotMsg RobotMsg_UnloadEmptyTray = new RobotMsg();

        #region PLC监听事件
        [ProcessMethod("", "PLC_NoticeEmptyTray", "PLC通知需要上空Tray", true)]
        public ProcessResponse PLC_NoticeEmptyTray(IOperationConfig config, IDevice device)
        {
            if (config.InputPara == null || config.InputPara.Count == 0)
            TrayTask task = new TrayTask();
            task.TaskType = TaskType.UnloadEmptyTrayToMachine;
            task.SourceDeviceId = device.Id;
            //task.SourceDeviceName = device.Name;
            task.Location = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray && u.DeviceOwner == device.Id);

            InsertTask(task);

            return new ProcessResponse(true);
        }

        [ProcessMethod("", "PLC_NoticeFullTray", "PLC通知满Tray需要取走", true)]
        public ProcessResponse PLC_NoticeFullTray(IOperationConfig config, IDevice device)
        {
            TrayTask task = new TrayTask();
            task.TaskType = TaskType.LoadFullTrayFromMachine;
            task.SourceDeviceId = device.Id;
            //task.SourceDeviceName = device.Name;
            task.Location = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.DeviceOwner == device.Id);

            InsertTask(task);

            return new ProcessResponse(true);
        }
        #endregion

        [ProcessMethod("OperationTest", "OperationDemo", "单步测试方法", true)]
        public ProcessResponse OperationDemo(IOperationConfig config, IDevice device)
        {
            string s = (-1).ToString("D2");
            string a = (1).ToString("D2");

            OperationTestConfig opConfig = config as OperationTestConfig;

            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.AGVId == opConfig.AGVId);
            if (bind == null)
                throw new ProcessException("未能获取指定AGV信息或AGV绑定的设备信息");

            switch (opConfig.TaskInfo.TaskType)
            {
                throw new ProcessException("上空Tray方法未配置输入参数", null);
            }

            bool isEmptyTrayNeed = config.InputPara[0] == 1;

            if (isEmptyTrayNeed)
            {
                var position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray && u.DeviceOwner == device.Id);

                machineEmptyTrayDict[position.PositionNo] = 0;

                if (!taskAssignedList.Any(u => u.PositionNo == position.PositionNo))
                {
                    taskAssignedList.Add(new TaskAssignInfo()
                    {
                        IsTaskNeed = true,
                        IsTaskAssgined = false,
                        PositionNo = position.PositionNo,
                    });
                }

                CheckEmptyTrayTask(position.PositionNo);
                case TaskType.LoadEmptyTrayToAGV:
                    LoadEmptyTrayToAGV(opConfig.TaskInfo, bind);
                    break;
                case TaskType.LoadFullTrayFromMachine:
                    LoadFullTrayFromMachine(opConfig.TaskInfo, bind);
                    break;
                case TaskType.UnloadEmptyTrayToMachine:
                    UnloadEmptyTrayToMachine(opConfig.TaskInfo, bind);
                    break;
                case TaskType.UnloadFullTrayToLine:
                    UnloadFullTrayToLine(opConfig.TaskInfo, bind);
                    break;
                default:
                    throw new ProcessException($"未能指定{opConfig.TaskInfo.TaskType.ToString()}的对应方法");
            }

            return new ProcessResponse(true);
        }

        private async void CheckEmptyTrayTask(int positionNo)
        {
            await Task.Run(() =>
            {
                var taskStatus = taskAssignedList.FirstOrDefault(u => u.PositionNo == positionNo);
        #region old
        //#region 空Tray上料
        ////[ProcessMethod("", "AGV_LoadEmptyTray", "AGV去往空Tray上料", true)]
        ////public ProcessResponse AGV_LoadEmptyTray(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

                if (taskStatus == null)
                    return;
        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置空Tray上料点");
        ////    }

                while (taskStatus.IsTaskNeed && !taskStatus.IsTaskAssgined)
                {
                    Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_UnloadEmptyTray;
        ////    bind.AGVDest = position.PositionCode;
        ////    bind.AGV.TaskOrder(position.PositionCode);

                    //if (!Config.AGVBindCollection.Any(b => b.TaskList.Any(t => t.MethodFunc.Method.Name == action.Method.Name)))
                    {
                        var bind = Config.AGVBindCollection.FirstOrDefault(u => u.UnitStatus == TaskStatus.Available);
                        if (bind != null)
                        {
                            var position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNo);
                            AGVTaskModel model_AGV = new AGVTaskModel(TaskAvailableLevel.Both, action, new AGVBindOpConfig(bind.Id, position));
                            AGVTaskModel model_Robot = new AGVTaskModel(TaskAvailableLevel.AGV, Robot_UnloadEmptyTray, new AGVBindOpConfig(bind.Id));
        ////    bind.AGVStatus = TaskStatus.Running;

                            bind.TaskList.Add(model_AGV);
                            bind.TaskList.Add(model_Robot);
        ////    return new ProcessResponse(true);
        ////}

                            taskStatus.IsTaskAssgined = true;
                            taskStatus.AgvId = bind.AGVId;
                        }
                    }

                    Thread.Sleep(300);
                }
            });
        }

        //[ProcessMethod("", "AGV_UnloadEmptyTray", "AGV去往卸载空Tray料位置", true)]
        public ProcessResponse AGV_UnloadEmptyTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);

            PathPosition position = (config as AGVBindOpConfig).Position;

            if (position == null)
            {
                throw new ProcessException("路径配置未设置空Tray下料点");
            }

            bind.AGVDest = position.PositionCode;
            bind.AGV.TaskOrder(position.PositionCode);

            bind.AGVStatus = TaskStatus.Running;

            return new ProcessResponse(true);
        }

        //[ProcessMethod("", "Robot_UnloadEmptyTray", "机器人运动至空Tray拍照位置", true)]
        public ProcessResponse Robot_UnloadEmptyTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray && u.PositionCode == bind.AGV.CurrentPosition);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置空Tray下料点");
            }

            taskAssignedList.RemoveAll(u => u.AgvId == bind.AGVId && u.PositionNo == position.PositionNo);

            bind.RobotStatus = TaskStatus.Running;
            bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.UnloadEmptyTraySnap, position.PositionNo);

            return new ProcessResponse(true);
        }

        //[ProcessMethod("", "Camera_UnloadEmptyTray", "相机确认空Tray卸载机器人位置调整", true)]
        //public ProcessResponse Camera_UnloadEmptyTray(IOperationConfig config, IDevice device)
        //public bool AGV_LoadEmptyTray(string bindId)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray);
        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置空Tray上料点");
        //    }

        //    if (bind.SetAGVStatus(TaskStatus.Running))
        //    {
        //        bind.AGVDest = position.PositionCode;
        //        bind.AGV.TaskOrder(position.PositionCode);

        //        return true;
        //    }
        //    else
        //    {
        //        return false;
        //    }
        //}

        ////[ProcessMethod("", "AfterEmptyTrayPositionArrived", "到达空Tray上料点", true)]
        ////public ProcessResponse AfterEmptyTrayPositionArrived(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置空Tray上料点", null);
        ////    }

        ////    if (bind.AGV.CurrentPosition != position.PositionCode)
        ////    {
        ////        throw new ProcessException("AGV尚未到达空Tray上料点", null);
        ////    }

        ////    bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.EmptyTray, 0);
        ////    bind.RobotStatus = TaskStatus.Running;

        ////    return new ProcessResponse(true);
        ////}

        //public void Robot_LoadEmptyTray(string bindId, PathPosition position)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
        //    //PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置空Tray上料点", null);
        //    }

        //    if (bind.AGV.CurrentPosition != position.PositionCode)
        //    {
        //        throw new ProcessException("AGV尚未到达空Tray上料点", null);
        //    }

        //    bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.EmptyTray, 0);
        //    bind.RobotStatus = TaskStatus.Running;
        //}

        ////[ProcessMethod("", "EmptyTrayReady", "空Tray上料完成", true)]
        ////public ProcessResponse EmptyTrayReady(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        ////    bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;

        ////    return new ProcessResponse(true);
        ////}
        //#endregion

        //#region 空Tray往机台下料
        ////bool isEmptyTrayNeed = false;
        ////bool isEmptyTrayTaskAssigned = false;
        //RobotMsg RobotMsg_UnloadEmptyTray = new RobotMsg();

        //private async void CheckUnloadEmptyTrayTask(int positionNo)
        //{
        //    await Task.Run(() =>
        //    {
        //        var taskStatus = taskAssignedList.FirstOrDefault(u => u.PositionNo == positionNo);

        //        if (taskStatus == null)
        //            return;

        //        while (taskStatus.IsTaskNeed && !taskStatus.IsTaskAssgined)
        //        {
        //            //Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_UnloadEmptyTray;

        //            //if (!Config.AGVBindCollection.Any(b => b.TaskList.Any(t => t.MethodName == "AGV_UnloadEmptyTray")))
        //            {
        //                var bind = Config.AGVBindCollection.FirstOrDefault(u => u.UnitStatus == TaskStatus.Available);
        //                if (bind != null)
        //                {
        //                    var position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNo);
        //                    //AGVTaskModel model_AGV = new AGVTaskModel(TaskAvailableLevel.Both, "AGV_UnloadEmptyTray", new AGVBindOpConfig(bind.Id, position));
        //                    //AGVTaskModel model_Robot = new AGVTaskModel(TaskAvailableLevel.AGV, "Robot_UnloadEmptyTray", new AGVBindOpConfig(bind.Id));

        //                    //bind.AddTask(model_AGV);
        //                    //bind.AddTask(model_Robot);

        //                    if (AGV_UnloadEmptyTray(bind.Id, position))
        //                    {
        //                        taskStatus.IsTaskAssgined = true;
        //                        taskStatus.AgvId = bind.AGVId;
        //                    }
        //                }
        //            }

        //            Thread.Sleep(WAITTIME);
        //        }
        //    });
        //}

        ////[ProcessMethod("", "AGV_UnloadEmptyTray", "AGV去往卸载空Tray料位置", true)]
        //public bool AGV_UnloadEmptyTray(string bindId, PathPosition position)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置空Tray下料点");
        //    }

        //    if (bind.SetAGVStatus(TaskStatus.Running))
        //    {
        //        bind.AGVDest = position.PositionCode;
        //        bind.AGV.TaskOrder(position.PositionCode);

        //        return true;
        //    }
        //    else
        //    {
        //        return false;
        //    }
        //}

        ////[ProcessMethod("", "Robot_UnloadEmptyTray", "机器人运动至空Tray拍照位置", true)]
        //public void Robot_UnloadEmptyTraySnap(string bindId, PathPosition position)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
        //    //PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray && u.PositionCode == bind.AGV.CurrentPosition);

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置空Tray下料点");
        //    }

        //    taskAssignedList.RemoveAll(u => u.AgvId == bind.AGVId && u.PositionNo == position.PositionNo);

        //    bind.RobotStatus = TaskStatus.Running;
        //    bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.UnloadEmptyTraySnap, position.PositionNo);
        //}

        ////[ProcessMethod("", "Camera_UnloadEmptyTray", "相机确认空Tray卸载机器人位置调整", true)]
        ////public ProcessResponse Camera_UnloadEmptyTray(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);

        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray);

        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置空Tray下料点");
        ////    }

        ////    if (bind.AGV.CurrentPosition != position.PositionCode)
        ////    {
        ////        throw new ProcessException("AGV当前未处于空Tray下料点");
        ////    }

        ////    PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

        ////    if (visionConfig == null)
        ////    {
        ////        throw new ProcessException("未配置该相机的空Tray下料点的视觉操作配置");
        ////    }

        ////    float x = 0;
        ////    float y = 0;
        ////    float angle = 0;

        ////    using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadEmptyTray"))
        ////    {
        ////        string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        ////        if (!_halconToolDict.ContainsKey(toolPath))
        ////        {
        ////            throw new ProcessException($"未配置Camera_UnloadEmptyTray的视觉算法路径");
        ////        }

        ////        var tool = _halconToolDict[toolPath];
        ////        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();

        ////        x = (float)tool.GetResultTuple("OUTPUT_X").D;
        ////        y = (float)tool.GetResultTuple("OUTPUT_Y").D;
        ////        angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
        ////    }

        ////    if (x <= 0 || y <= 0)
        ////    {
        ////        throw new ProcessException("Camera_UnloadEmptyTray视觉计算获取点位不可小于0");
        ////    }

        ////    float dx = visionConfig.StandardPoint.X - x;
        ////    float dy = visionConfig.StandardPoint.Y - y;

        ////    HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

        ////    bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });

        ////    return new ProcessResponse(true);
        ////}

        //public ProcessResponse Camera_UnloadEmptyTray(string robotId, int positionNum)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);

        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNum);

        //    if (position == null)
        //    {
@@ -586,233 +456,242 @@
        //        throw new ProcessException("AGV当前未处于空Tray下料点");
        //    }

        //    PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
        //    float adjust_X = 0.0f;
        //    float adjust_Y = 0.0f;
        //    float adjust_Angle = 0.0f;

        //    if (visionConfig == null)
        //    if (Config.IsEnableVisionGuide)
        //    {
        //        throw new ProcessException("未配置该相机的空Tray下料点的视觉操作配置");
        //    }
        //        PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

        //    float x = 0;
        //    float y = 0;
        //    float angle = 0;

        //    using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadEmptyTray"))
        //    {
        //        string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        //        if (!_halconToolDict.ContainsKey(toolPath))
        //        if (visionConfig == null)
        //        {
        //            throw new ProcessException($"未配置Camera_UnloadEmptyTray的视觉算法路径");
        //            throw new ProcessException("未配置该相机的空Tray下料点的视觉操作配置");
        //        }

        //        var tool = _halconToolDict[toolPath];
        //        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 = 0;
        //        float y = 0;
        //        float angle = 0;

        //        x = (float)tool.GetResultTuple("OUTPUT_X").D;
        //        y = (float)tool.GetResultTuple("OUTPUT_Y").D;
        //        angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
        //        using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadEmptyTray"))
        //        {
        //            string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        //            if (!_halconToolDict.ContainsKey(toolPath))
        //            {
        //                throw new ProcessException($"未配置Camera_UnloadEmptyTray的视觉算法路径");
        //            }

        //            var tool = _halconToolDict[toolPath];
        //            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();

        //            x = (float)tool.GetResultTuple("OUTPUT_X").D;
        //            y = (float)tool.GetResultTuple("OUTPUT_Y").D;
        //            angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
        //        }

        //        if (x <= 0 || y <= 0)
        //        {
        //            throw new ProcessException("Camera_UnloadEmptyTray视觉计算获取点位不可小于0");
        //        }

        //        float dx = visionConfig.StandardPoint.X - x;
        //        float dy = visionConfig.StandardPoint.Y - y;

        //        HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

        //        adjust_X = (float)dx_Robot.D;
        //        adjust_Y = (float)dy_Robot.D;
        //        adjust_Angle = visionConfig.StandardPoint.Angle - angle;
        //    }

        //    if (x <= 0 || y <= 0)
        //    {
        //        throw new ProcessException("Camera_UnloadEmptyTray视觉计算获取点位不可小于0");
        //    }

        //    float dx = visionConfig.StandardPoint.X - x;
        //    float dy = visionConfig.StandardPoint.Y - y;

        //    HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

        //    bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
        //    //bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
        //    RobotMsg_UnloadEmptyTray.Action = RobotMsgAction.Unload;
        //    RobotMsg_UnloadEmptyTray.Para1 = RobotMsgParas.EmptyTray;
        //    RobotMsg_UnloadEmptyTray.Para2 = position.PositionNo;
        //    RobotMsg_UnloadEmptyTray.Datas = new List<float>() { adjust_X, adjust_Y, 0, adjust_Angle }.ConvertAll(s => s.ToString()).ToList();
        //    bind.Robot.SendMsg(RobotMsg_UnloadEmptyTray, true);

        //    return new ProcessResponse(true);
        //}
        //#endregion

        public ProcessResponse Camera_UnloadEmptyTray(string robotId, int positionNum)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
        //#region 从机台上满Tray
        ////bool isFullTrayNeed = false;
        ////bool isFullTrayTaskAssigned = false;
        //RobotMsg RobotMsg_LoadFullTray = new RobotMsg();

            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNum);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置空Tray下料点");
            }

            if (bind.AGV.CurrentPosition != position.PositionCode)
            {
                throw new ProcessException("AGV当前未处于空Tray下料点");
            }

            PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

            if (visionConfig == null)
            {
                throw new ProcessException("未配置该相机的空Tray下料点的视觉操作配置");
            }

            float x = 0;
            float y = 0;
            float angle = 0;

            using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadEmptyTray"))
            {
                string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
                if (!_halconToolDict.ContainsKey(toolPath))
                {
                    throw new ProcessException($"未配置Camera_UnloadEmptyTray的视觉算法路径");
                }

                var tool = _halconToolDict[toolPath];
                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();

                x = (float)tool.GetResultTuple("OUTPUT_X").D;
                y = (float)tool.GetResultTuple("OUTPUT_Y").D;
                angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
            }

            if (x <= 0 || y <= 0)
            {
                throw new ProcessException("Camera_UnloadEmptyTray视觉计算获取点位不可小于0");
            }

            float dx = visionConfig.StandardPoint.X - x;
            float dy = visionConfig.StandardPoint.Y - y;

            HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

            //bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
            RobotMsg_UnloadEmptyTray.Action = RobotMsgAction.Unload;
            RobotMsg_UnloadEmptyTray.Para1 = RobotMsgParas.EmptyTray;
            RobotMsg_UnloadEmptyTray.Para2 = position.PositionNo;
            RobotMsg_UnloadEmptyTray.Datas = new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle }.ConvertAll(s => s.ToString()).ToList();
            bind.Robot.SendMsg(RobotMsg_UnloadEmptyTray, true);

            return new ProcessResponse(true);
        }
        #endregion

        #region 从机台上满Tray
        //bool isFullTrayNeed = false;
        //bool isFullTrayTaskAssigned = false;
        RobotMsg RobotMsg_LoadFullTray = new RobotMsg();

        [ProcessMethod("", "PLC_NoticeFullTray", "PLC通知满Tray需要取走", true)]
        public ProcessResponse PLC_NoticeFullTray(IOperationConfig config, IDevice device)
        {
            if (config.InputPara == null || config.InputPara.Count == 0)
            {
                throw new ProcessException("上空Tray方法未配置输入参数", null);
            }

            bool isFullTrayNeed = config.InputPara[0] == 1;
            if (isFullTrayNeed)
            {
                var position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.DeviceOwner == device.Id);

                machineFullTrayDict[position.PositionNo] = Config.Machine_FullTrayNum;

                if (!taskAssignedList.Any(u => u.PositionNo == position.PositionNo))
                {
                    taskAssignedList.Add(new TaskAssignInfo()
                    {
                        IsTaskNeed = true,
                        IsTaskAssgined = false,
                        PositionNo = position.PositionNo,
                    });
                }

                CheckFullTrayTask(position.PositionNo);
            }

            return new ProcessResponse(true);
        }

        private async void CheckFullTrayTask(int positionNo)
        {
            await Task.Run(() =>
            {
                var taskStatus = taskAssignedList.FirstOrDefault(u => u.PositionNo == positionNo);

                if (taskStatus == null)
                    return;

                while (taskStatus.IsTaskNeed && !taskStatus.IsTaskAssgined)
                {
                    Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_LoadFullTray;

                    //if (!Config.AGVBindCollection.Any(b => b.TaskList.Any(t => t.MethodFunc.Method.Name == action.Method.Name)))
                    {
                        var bind = Config.AGVBindCollection.FirstOrDefault(u => u.UnitStatus == TaskStatus.Available);
                        if (bind != null)
                        {
                            var position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNo);
                            AGVTaskModel model_AGV = new AGVTaskModel(TaskAvailableLevel.Both, action, new AGVBindOpConfig(bind.Id, position));
                            AGVTaskModel model_Robot = new AGVTaskModel(TaskAvailableLevel.AGV, Robot_LoadFullTray, new AGVBindOpConfig(bind.Id));

                            bind.TaskList.Add(model_AGV);
                            bind.TaskList.Add(model_Robot);

                            taskStatus.IsTaskAssgined = true;
                            taskStatus.AgvId = bind.AGVId;
                        }
                    }

                    Thread.Sleep(300);
                }
            });
        }

        [ProcessMethod("", "AGV_LoadFullTray", "AGV去往满Tray上料位置", true)]
        public ProcessResponse AGV_LoadFullTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);

            PathPosition position = (config as AGVBindOpConfig).Position;

            if (position == null)
            {
                throw new ProcessException("路径配置未设置满Tray上料点");
            }

            bind.AGVDest = position.PositionCode;
            bind.AGV.TaskOrder(position.PositionCode);

            bind.AGVStatus = TaskStatus.Running;

            return new ProcessResponse(true);
        }

        [ProcessMethod("", "Robot_LoadFullTray", "机器人运动至满Tray拍照位置", true)]
        public ProcessResponse Robot_LoadFullTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.PositionCode == bind.AGV.CurrentPosition);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置满Tray上料点");
            }

            //if (bind.AGV.CurrentPosition != position.PositionCode)
            //{
            //    throw new ProcessException("AGV当前未处于满Tray上料点");
            //}

            bind.RobotStatus = TaskStatus.Running;
            bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LoadFullTraySnap, position.PositionNo);

            return new ProcessResponse(true);
        }

        //[ProcessMethod("", "Camera_LoadFullTray", "相机确认满Tray上料机器人位置调整", true)]
        //public ProcessResponse Camera_LoadFullTray(IOperationConfig config, IDevice device)
        //private async void CheckFullTrayTask(int positionNo)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        //    await Task.Run(() =>
        //    {
        //        var taskStatus = taskAssignedList.FirstOrDefault(u => u.PositionNo == positionNo);

        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray);
        //        if (taskStatus == null)
        //            return;

        //        while (taskStatus.IsTaskNeed && !taskStatus.IsTaskAssgined)
        //        {
        //            //Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_LoadFullTray;

        //            //if (!Config.AGVBindCollection.Any(b => b.TaskList.Any(t => t.MethodFunc.Method.Name == action.Method.Name)))
        //            {
        //                var bind = Config.AGVBindCollection.FirstOrDefault(u => u.UnitStatus == TaskStatus.Available);
        //                if (bind != null)
        //                {
        //                    var position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNo);
        //                    //AGVTaskModel model_AGV = new AGVTaskModel(TaskAvailableLevel.Both, "AGV_LoadFullTray", new AGVBindOpConfig(bind.Id, position));
        //                    //AGVTaskModel model_Robot = new AGVTaskModel(TaskAvailableLevel.AGV, "Robot_LoadFullTray", new AGVBindOpConfig(bind.Id));

        //                    //bind.AddTask(model_AGV);
        //                    //bind.AddTask(model_Robot);

        //                    if (AGV_LoadFullTray(bind.Id, position))
        //                    {
        //                        taskStatus.IsTaskAssgined = true;
        //                        taskStatus.AgvId = bind.AGVId;
        //                    }
        //                }
        //            }

        //            Thread.Sleep(300);
        //        }
        //    });
        //}

        ////[ProcessMethod("", "AGV_LoadFullTray", "AGV去往满Tray上料位置", true)]
        //public bool AGV_LoadFullTray(string bindId, PathPosition position)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);

        //    //PathPosition position = (config as AGVBindOpConfig).Position;

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置满Tray上料点");
        //    }

        //    if (bind.SetAGVStatus(TaskStatus.Running))
        //    {
        //        bind.AGVDest = position.PositionCode;
        //        bind.AGV.TaskOrder(position.PositionCode);

        //        return true;
        //    }
        //    else
        //    {
        //        return false;
        //    }

        //    //return new ProcessResponse(true);
        //}

        ////[ProcessMethod("", "Robot_LoadFullTray", "机器人运动至满Tray拍照位置", true)]
        ////public ProcessResponse Robot_LoadFullTray(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.PositionCode == bind.AGV.CurrentPosition);

        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置满Tray上料点");
        ////    }

        ////    //if (bind.AGV.CurrentPosition != position.PositionCode)
        ////    //{
        ////    //    throw new ProcessException("AGV当前未处于满Tray上料点");
        ////    //}

        ////    bind.RobotStatus = TaskStatus.Running;
        ////    bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LoadFullTraySnap, position.PositionNo);

        ////    return new ProcessResponse(true);
        ////}

        //public void Robot_LoadFullTraySnap(string bindId, PathPosition position)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
        //    //PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.PositionCode == bind.AGV.CurrentPosition);

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置满Tray上料点");
        //    }

        //    //if (bind.AGV.CurrentPosition != position.PositionCode)
        //    //{
        //    //    throw new ProcessException("AGV当前未处于满Tray上料点");
        //    //}

        //    bind.RobotStatus = TaskStatus.Running;
        //    bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LoadFullTraySnap, position.PositionNo);
        //}

        ////[ProcessMethod("", "Camera_LoadFullTray", "相机确认满Tray上料机器人位置调整", true)]
        ////public ProcessResponse Camera_LoadFullTray(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);

        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray);

        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置满Tray上料点");
        ////    }

        ////    if (bind.AGV.CurrentPosition != position.PositionCode)
        ////    {
        ////        throw new ProcessException("AGV当前未处于满Tray上料点");
        ////    }

        ////    PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

        ////    if (visionConfig == null)
        ////    {
        ////        throw new ProcessException("未配置该相机的满Tray上料点的视觉操作配置");
        ////    }

        ////    float x = 0;
        ////    float y = 0;
        ////    float angle = 0;

        ////    using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_LoadFullTray"))
        ////    {
        ////        string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        ////        if (!_halconToolDict.ContainsKey(toolPath))
        ////        {
        ////            throw new ProcessException($"未配置Camera_LoadFullTray的视觉算法路径");
        ////        }

        ////        var tool = _halconToolDict[toolPath];
        ////        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();

        ////        x = (float)tool.GetResultTuple("OUTPUT_X").D;
        ////        y = (float)tool.GetResultTuple("OUTPUT_Y").D;
        ////        angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
        ////    }

        ////    if (x <= 0 || y <= 0)
        ////    {
        ////        throw new ProcessException("Camera_LoadFullTray视觉计算获取点位不可小于0");
        ////    }

        ////    float dx = visionConfig.StandardPoint.X - x;
        ////    float dy = visionConfig.StandardPoint.Y - y;

        ////    HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

        ////    bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });

        ////    return new ProcessResponse(true);
        ////}

        //public ProcessResponse Camera_LoadFullTray(string robotId, int positionNum)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);

        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNum);

        //    if (position == null)
        //    {
@@ -824,256 +703,239 @@
        //        throw new ProcessException("AGV当前未处于满Tray上料点");
        //    }

        //    PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
        //    float adjust_X = 0.0f;
        //    float adjust_Y = 0.0f;
        //    float adjust_Angle = 0.0f;

        //    if (visionConfig == null)
        //    if (Config.IsEnableVisionGuide)
        //    {
        //        throw new ProcessException("未配置该相机的满Tray上料点的视觉操作配置");
        //    }
        //        PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

        //    float x = 0;
        //    float y = 0;
        //    float angle = 0;

        //    using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_LoadFullTray"))
        //    {
        //        string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        //        if (!_halconToolDict.ContainsKey(toolPath))
        //        if (visionConfig == null)
        //        {
        //            throw new ProcessException($"未配置Camera_LoadFullTray的视觉算法路径");
        //            throw new ProcessException("未配置该相机的满Tray上料点的视觉操作配置");
        //        }

        //        var tool = _halconToolDict[toolPath];
        //        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 = 0;
        //        float y = 0;
        //        float angle = 0;

        //        x = (float)tool.GetResultTuple("OUTPUT_X").D;
        //        y = (float)tool.GetResultTuple("OUTPUT_Y").D;
        //        angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
        //        using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_LoadFullTray"))
        //        {
        //            string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        //            if (!_halconToolDict.ContainsKey(toolPath))
        //            {
        //                throw new ProcessException($"未配置Camera_LoadFullTray的视觉算法路径");
        //            }

        //            var tool = _halconToolDict[toolPath];
        //            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();

        //            x = (float)tool.GetResultTuple("OUTPUT_X").D;
        //            y = (float)tool.GetResultTuple("OUTPUT_Y").D;
        //            angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
        //        }

        //        if (x <= 0 || y <= 0)
        //        {
        //            throw new ProcessException("Camera_LoadFullTray视觉计算获取点位不可小于0");
        //        }

        //        float dx = visionConfig.StandardPoint.X - x;
        //        float dy = visionConfig.StandardPoint.Y - y;

        //        HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

        //        adjust_X = (float)dx_Robot.D;
        //        adjust_Y = (float)dy_Robot.D;
        //        adjust_Angle = visionConfig.StandardPoint.Angle - angle;
        //    }

        //    if (x <= 0 || y <= 0)
        //    {
        //        throw new ProcessException("Camera_LoadFullTray视觉计算获取点位不可小于0");
        //    }

        //    float dx = visionConfig.StandardPoint.X - x;
        //    float dy = visionConfig.StandardPoint.Y - y;

        //    HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);

        //    bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
        //    //bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
        //    RobotMsg_LoadFullTray.Action = RobotMsgAction.Load;
        //    RobotMsg_LoadFullTray.Para1 = RobotMsgParas.FullTray;
        //    RobotMsg_LoadFullTray.Para2 = position.PositionNo;
        //    RobotMsg_LoadFullTray.Datas = new List<float>() { adjust_X, adjust_Y, 0, adjust_Angle }.ConvertAll(s => s.ToString()).ToList();
        //    bind.Robot.SendMsg(RobotMsg_LoadFullTray, true);

        //    return new ProcessResponse(true);
        //}
        //#endregion

        public ProcessResponse Camera_LoadFullTray(string robotId, int positionNum)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
        //#region 满Tray产线下料
        ////[ProcessMethod("", "AGV_UnloadFullTray", "AGV去往卸载满Tray料", true)]
        ////public ProcessResponse AGV_UnloadFullTray(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);

            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNum);
        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置满Tray下料点");
        ////    }

            if (position == null)
            {
                throw new ProcessException("路径配置未设置满Tray上料点");
            }
        ////    bind.AGVDest = position.PositionCode;
        ////    bind.AGV.TaskOrder(position.PositionCode);

            if (bind.AGV.CurrentPosition != position.PositionCode)
            {
                throw new ProcessException("AGV当前未处于满Tray上料点");
            }
        ////    bind.AGVStatus = TaskStatus.Running;

            PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
        ////    return new ProcessResponse(true);
        ////}

            if (visionConfig == null)
            {
                throw new ProcessException("未配置该相机的满Tray上料点的视觉操作配置");
            }
        //public bool AGV_UnloadFullTray(string bindId)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);

            float x = 0;
            float y = 0;
            float angle = 0;
        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置满Tray下料点");
        //    }

            using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_LoadFullTray"))
            {
                string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
                if (!_halconToolDict.ContainsKey(toolPath))
                {
                    throw new ProcessException($"未配置Camera_LoadFullTray的视觉算法路径");
                }
        //    if (bind.SetAGVStatus(TaskStatus.Running))
        //    {
        //        bind.AGVDest = position.PositionCode;
        //        bind.AGV.TaskOrder(position.PositionCode);

                var tool = _halconToolDict[toolPath];
                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();
        //        return true;
        //    }
        //    else
        //    {
        //        return false;
        //    }
        //}

                x = (float)tool.GetResultTuple("OUTPUT_X").D;
                y = (float)tool.GetResultTuple("OUTPUT_Y").D;
                angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
            }
        ////[ProcessMethod("", "Robot_UnloadFullTray", "机器人卸载满Tray", true)]
        ////public ProcessResponse Robot_UnloadFullTray(IOperationConfig config, IDevice device)
        ////{
        ////    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
        ////    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);

            if (x <= 0 || y <= 0)
            {
                throw new ProcessException("Camera_LoadFullTray视觉计算获取点位不可小于0");
            }
        ////    if (position == null)
        ////    {
        ////        throw new ProcessException("路径配置未设置满Tray下料点");
        ////    }

            float dx = visionConfig.StandardPoint.X - x;
            float dy = visionConfig.StandardPoint.Y - y;
        ////    if (bind.AGV.CurrentPosition != position.PositionCode)
        ////    {
        ////        throw new ProcessException("AGV当前未处于满Tray下料点");
        ////    }

            HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);
        ////    bind.RobotStatus = TaskStatus.Running;
        ////    bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LineSnap, position.PositionNo);

            //bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
            RobotMsg_LoadFullTray.Action = RobotMsgAction.Load;
            RobotMsg_LoadFullTray.Para1 = RobotMsgParas.FullTray;
            RobotMsg_LoadFullTray.Para2 = position.PositionNo;
            RobotMsg_LoadFullTray.Datas = new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle }.ConvertAll(s => s.ToString()).ToList();
            bind.Robot.SendMsg(RobotMsg_LoadFullTray, true);
        ////    return new ProcessResponse(true);
        ////}

            return new ProcessResponse(true);
        }
        //public void Robot_UnloadFullTraySnap(string bindId, PathPosition position)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
        //    //PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置满Tray下料点");
        //    }

        //    if (bind.AGV.CurrentPosition != position.PositionCode)
        //    {
        //        throw new ProcessException("AGV当前未处于满Tray下料点");
        //    }

        //    bind.RobotStatus = TaskStatus.Running;
        //    bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LineSnap, position.PositionNo);
        //    //LogAsync(DateTime.Now, "Robot运动至下满Tray拍照", "");
        //}

        ////[ProcessMethod("", "Camera_UnloadFullTray", "相机操作卸载满Tray", true)]
        ////public ProcessResponse Camera_UnloadFullTray(IOperationConfig config, IDevice device)
        //public ProcessResponse Camera_UnloadFullTray(string robotId, int positionNum)
        //{
        //    var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
        //    PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray && u.PositionNo == positionNum);

        //    if (position == null)
        //    {
        //        throw new ProcessException("路径配置未设置满Tray下料点");
        //    }

        //    if (bind.AGV.CurrentPosition != position.PositionCode)
        //    {
        //        throw new ProcessException("AGV当前未处于满Tray下料点");
        //    }

        //    //float adjust_X = 0.0f;
        //    //float adjust_Y = 0.0f;
        //    //float adjust_Angle = 0.0f;
        //    bool isLineReady = false;

        //    if (Config.IsEnableVisionGuide)
        //    {
        //        PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

        //        if (visionConfig == null)
        //        {
        //            throw new ProcessException("未配置该相机的满Tray下料点的视觉操作配置");
        //        }


        //        int reTryTime = Config.LineBusyRetryTimes;

        //        do
        //        {
        //            using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadFullTray"))
        //            {
        //                string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
        //                if (!_halconToolDict.ContainsKey(toolPath))
        //                {
        //                    throw new ProcessException($"未配置Camera_UnloadFullTray的视觉算法路径");
        //                }

        //                _halconToolDict[toolPath].SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_Result", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
        //                _halconToolDict[toolPath].RunProcedure();

        //                isLineReady = _halconToolDict[toolPath].GetResultTuple("OUTPUT_Result").I == 1;
        //            }

        //            if (!isLineReady)
        //            {
        //                Thread.Sleep(Config.LineBusyWaitInterval * 1000);
        //                reTryTime--;
        //            }
        //            else
        //            {
        //                reTryTime = 0;
        //            }
        //        } while (reTryTime > 0);

        //        //if (!isLineReady)
        //        //{
        //        //    bind.Robot.SendMsg(RobotMsgType.Send, -1, true, RobotMsgAction.State, RobotMsgParas.LineSnap, new List<string>() { "-1" });
        //        //    throw new ProcessException("产线忙，等待超时");
        //        //}
        //        //else
        //        //{
        //        //    bind.Robot.SendMsg(RobotMsgType.Send, -1, true, RobotMsgAction.State, RobotMsgParas.LineSnap, new List<string>() { "1" });
        //        //}
        //    }
        //    else
        //    {
        //        isLineReady = true;
        //    }

        //    if (isLineReady)
        //    {
        //        bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.FullTray, position.PositionNo);
        //    }
        //    else
        //    {
        //        bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.Home, position.PositionNo);
        //    }

        //    return new ProcessResponse(true);
        //}
        //#endregion
        #endregion

        #region 满Tray产线下料
        [ProcessMethod("", "AGV_UnloadFullTray", "AGV去往卸载满Tray料", true)]
        public ProcessResponse AGV_UnloadFullTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置满Tray下料点");
            }

            bind.AGVDest = position.PositionCode;
            bind.AGV.TaskOrder(position.PositionCode);

            bind.AGVStatus = TaskStatus.Running;

            return new ProcessResponse(true);
        }

        [ProcessMethod("", "Robot_UnloadFullTray", "机器人卸载满Tray", true)]
        public ProcessResponse Robot_UnloadFullTray(IOperationConfig config, IDevice device)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置满Tray下料点");
            }

            if (bind.AGV.CurrentPosition != position.PositionCode)
            {
                throw new ProcessException("AGV当前未处于满Tray下料点");
            }

            bind.RobotStatus = TaskStatus.Running;
            bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LineSnap, position.PositionNo);

            return new ProcessResponse(true);
        }

        //[ProcessMethod("", "Camera_UnloadFullTray", "相机操作卸载满Tray", true)]
        //public ProcessResponse Camera_UnloadFullTray(IOperationConfig config, IDevice device)
        public ProcessResponse Camera_UnloadFullTray(string robotId, int positionNum)
        {
            var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
            PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray && u.PositionNo == positionNum);

            if (position == null)
            {
                throw new ProcessException("路径配置未设置满Tray下料点");
            }

            if (bind.AGV.CurrentPosition != position.PositionCode)
            {
                throw new ProcessException("AGV当前未处于满Tray下料点");
            }

            PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);

            if (visionConfig == null)
            {
                throw new ProcessException("未配置该相机的满Tray下料点的视觉操作配置");
            }

            bool isLineReady = false;
            int reTryTime = Config.LineBusyRetryTimes;

            do
            {
                using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadFullTray"))
                {
                    string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
                    if (!_halconToolDict.ContainsKey(toolPath))
                    {
                        throw new ProcessException($"未配置Camera_UnloadFullTray的视觉算法路径");
                    }

                    _halconToolDict[toolPath].SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_Result", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
                    _halconToolDict[toolPath].RunProcedure();

                    isLineReady = _halconToolDict[toolPath].GetResultTuple("OUTPUT_Result").I == 1;
                }

                if (!isLineReady)
                {
                    Thread.Sleep(Config.LineBusyWaitInterval * 1000);
                    reTryTime--;
                }
                else
                {
                    reTryTime = 0;
                }
            } while (reTryTime > 0);

            //if (!isLineReady)
            //{
            //    bind.Robot.SendMsg(RobotMsgType.Send, -1, true, RobotMsgAction.State, RobotMsgParas.LineSnap, new List<string>() { "-1" });
            //    throw new ProcessException("产线忙，等待超时");
            //}
            //else
            //{
            //    bind.Robot.SendMsg(RobotMsgType.Send, -1, true, RobotMsgAction.State, RobotMsgParas.LineSnap, new List<string>() { "1" });
            //}

            if (isLineReady)
            {
                bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.FullTray, position.PositionNo);
            }
            else
            {
                bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.Home, position.PositionNo);
            }

            return new ProcessResponse(true);
        }
        #endregion
    }

    public class AGVBindOpConfig : OperationConfigBase
    {
        public string BindId { get; set; }
        public PathPosition Position { get; set; }

        public AGVBindOpConfig() { }

        public AGVBindOpConfig(string bindId, PathPosition position = null)
        {
            BindId = bindId;
            Position = position;
        }
    }

    public class TaskAssignInfo
    {
        public int PositionNo { get; set; }

        public bool IsTaskNeed { get; set; } = false;

        public bool IsTaskAssgined { get; set; } = false;

        public string AgvId { get; set; }
    }
}