RCS / RCS_Differential_Slam_Changsha · Files

using RCS.Model.ClientMoel;
using RCS.Model.Comm;
using RCS.Model.Entity;
using RCS.WinClient.Common;
using RCS.WinClient.PLC;
using System.Data;


namespace RCS.WinClient.Service
{
    public class GetNextPoint
    {
        /// <summary>
        /// 为小车获取后续动作点
        /// </summary>
        /// <param name="">子任务信息</param>
        /// <param name="agv">小车信息</param>
        /// <returns></returns>sonTask
        public static RcsData GetRcsData(Base_SubTask subTask, Base_Agv agv)
        {
            try
            {
                var rcsData = new RcsData(subTask.SubTaskNo, agv.AgvTask);
                rcsData.ActionType = subTask.ActionType;
                rcsData.Height = subTask.Height;
                rcsData.Angle = subTask.Angle;
                rcsData.RCSToAGV = agv.RCSToAGV;
                var currentPathPoint = agv.PathPointList.FirstOrDefault(x => x.Barcode == agv.Barcode);
                if (currentPathPoint == null)
                {
                    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"当前地标【{agv.Barcode}】不在路径中！");
                    return rcsData;
                }
                //if (currentPathPoint.IsCorner)
                //{
                //    currentPathPoint.Point.LockedAgv = agv;
                //    var rcsDataDetail = GetRcsDataDetail(agv, subTask, currentPathPoint);
                //    rcsData.Details.Add(rcsDataDetail);
                //    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"当前点【{agv.Barcode}】是拐点，等车走过拐点再发新路径！");
                //    return rcsData;
                //}
                //储存此过程中添加的lock点;
                var lockedPoint = new List<Base_Point>();

                //找到小车后面的路径点，每次包含当前点位在内，最多发5个点位
                var pathList = agv.PathPointList.OrderBy(t => t.SerialNo).Take(5).ToList();

                //判断后面的点位可以否能去，能去就加入motionList中。
                foreach (Base_PathPoint pathPoint in pathList)
                {
                    //如果第5个点位是走弧点就不发点位
                    if (agv.AgvType == EnumMsg.AgvType.叉车 && rcsData.Details.Count == 4 && pathPoint.PathPointType == EnumMsg.PointType.走弧点)
                    {
                        break;
                    }

                    var point = pathPoint.Point;
                    var nextPoint = subTask.PathPointList.FirstOrDefault(x => x.SerialNo > pathPoint.SerialNo);

                    //if(nextPoint!=null){
                    //    if( App.StationList.Find(x => x.PreBarcode == point.Barcode && (x.Name.StartsWith("A02") || x.Name.In("A0101", "A0102"))) is Base_Station _station
                    //        && App.AgvList.Any(x=>x.Barcode== _station.Barcode)
                    //        )
                    //    {
                    //        //App.ExFile.MessageBusinessErr("GetRcsData", $"agv{agv.AgvNo}避让A区域站台{}中");
                    //        break;
                    //    }

                    //}
                    // pathPoint.PathDirection是当前点位往下个点位走的方向，上个点位往当前点位走的方向要取 lastPathPoint.PathDirection
                    //var previousPathPoint = subTask.PathPointList.Where(t => t.SerialNo < pathPoint.SerialNo).OrderByDescending(t => t.SerialNo).FirstOrDefault();

                    //如果没锁定的点位，就需要判断是否能去，锁定的路径就直接下发。
                    if (point.LockedAgv != agv)
                    {
                        //已经被其他车锁定，就不能去
                        if (point.LockedAgv != null)
                        {
                            AvoidTask(agv, point.LockedAgv);
                            break;
                        }

                        #region 特殊点位的处理

                        //只能发送旋转点
                        if (lockedPoint.FindAll(x => x.PointType == EnumMsg.PointType.整体旋转点).Count == 2)
                        {
                            break;
                        }

                        if (lockedPoint.Exists(x => x.PointType == EnumMsg.PointType.整体旋转点) && agv.Barcode.Point().Data.PointType != EnumMsg.PointType.整体旋转点)
                        {
                            break;
                        }

                        //站台有车，附属点也不能去
                        if (pathPoint.Point.PointType == EnumMsg.PointType.站台附属点)
                        {
                            var prePoint = App.PointList.Find(t => t.PreBarcode == pathPoint.Barcode);
                            if (prePoint != null && prePoint.LockedAgv != null && prePoint.LockedAgv != agv && prePoint.Barcode == subTask.EndBarcode)
                            {
                                AvoidTask(agv, prePoint.LockedAgv);
                                break;
                            }
                        }


                        #endregion

                        #region 路径的防撞检测
                        if (pathPoint.PreviousPathDirection != EnumMsg.Direction.无方向)
                        {
                            //检查前面是否有车，是否不能过去
                            if (CheckCrash(pathPoint, agv, pathPoint.PreviousPathDirection, out Base_Agv? crashAGV))
                            {
                                AvoidTask(agv, crashAGV);
                                break;
                            }
                            //如果是拐点，就要检查拐点的两侧是否有车，过去是否会撞车
                            if (pathPoint.IsCorner)
                            {
                                //如果会撞击就不能下发后面的点位
                                if (CheckCrash(pathPoint, agv, pathPoint.PathDirection, out Base_Agv? frontAGV))
                                {
                                    AvoidTask(agv, frontAGV);
                                    break;
                                }

                                #region 叉车暂时不考虑，拐弯的时候撞车尾，因为不会这么规划点位。所以先注释。
                                //var CheckDirection1 = EnumMsg.Direction.X正方向;
                                //var CheckDirection2 = EnumMsg.Direction.X负方向;
                                //if (previousPathPoint.PathDirection == EnumMsg.Direction.X正方向 || previousPathPoint.PathDirection == EnumMsg.Direction.X负方向)
                                //{
                                //    CheckDirection1 = EnumMsg.Direction.Y正方向;
                                //    CheckDirection2 = EnumMsg.Direction.Y负方向;
                                //}
                                ////如果检查的方向和前进的方向不一致，就是检车车尾会不会撞，最多预留2个车头长度
                                //if (pathPoint.PathDirection != CheckDirection1)
                                //{
                                //    minDistance = agv.HeadLength  * 2;
                                //}
                                ////如果会撞击就不能下发后面的点位
                                //if (CheckCrash(pathPoint, agv, CheckDirection1, minDistance, out Base_Agv? frontAGV))
                                //{
                                //    AvoidTask(agv, frontAGV);
                                //    break;
                                //}
                                ////如果检查的方向和前进的方向不一致，就是检车车尾会不会撞，最多预留2个车头长度
                                //if (pathPoint.PathDirection != CheckDirection2)
                                //{
                                //    minDistance = agv.HeadLength  * 2;
                                //}
                                ////如果会撞击就不能下发后面的点位
                                //if (CheckCrash(pathPoint, agv, CheckDirection2, minDistance, out Base_Agv? lastAGV))
                                //{
                                //    AvoidTask(agv, lastAGV);
                                //    break;
                                //}
                                #endregion
                            }
                        }

                        #endregion

                        #region 互斥管控,当前区域只许一辆车出入

                        if (point.RegionName.Contains("Mutex"))
                        {
                            var mutexPointList = App.PointList.FindAll(x => x.RegionName == point.RegionName);
                            bool areaResult = MutexAreaContrl(mutexPointList, pathPoint, agv, out Base_Agv? crashAGV);
                            if (areaResult)
                            {
                                AvoidTask(agv, crashAGV);
                                //if (rcsData.Details.Count == 1)
                                //{
                                //    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "互斥区域的流量管控；");
                                //}
                                break;
                            }
                        }

                        #endregion 互斥管控,当前区域只许一辆车出入

                        # region 当前区域只许同向AGV通行
                        if (point.RegionName.Contains("Direction"))
                        {
                            var areaPointList = App.PointList.FindAll(x => x.RegionName == point.RegionName);
                            bool areaResult = DirectionAreaControl(areaPointList, pathPoint, agv, out Base_Agv? crashAGV);
                            if (areaResult)
                            {
                                AvoidTask(agv, crashAGV);
                                //if (rcsData.Details.Count == 1)
                                //{
                                //    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "互斥区域的流量管控；");
                                //}
                                break;
                            }
                        }
                        #endregion 

                        #region 注释 整体旋转点托盘旋转

                        //路径中有需要旋转的旋转点，停
                        //if (agv.heightState == EnumMsg.HeightState.高位)
                        //{
                        //    if (lockedPoint.Exists(a => a.pointType == EnumMsg.PointType.整体旋转点) &&
                        //        pathList.Find(b => b.point.strBarcode == lockedPoint.Last(a => a.pointType == EnumMsg.PointType.整体旋转点).strBarcode
                        //    && b.pathPointType == EnumMsg.PointType.托盘旋转点) != null
                        //    && currentPathPoint.pathPointType != EnumMsg.PointType.托盘旋转点) break;
                        //    PathPoint nextPoint = ComnMethod.GetNextPathPoint(agv, currentPathPoint);
                        //    if (currentPathPoint.pathPointType == EnumMsg.PointType.托盘旋转点 && nextPoint != null)
                        //    {
                        //        if (nextPoint.isCorner)
                        //        {
                        //            if (nextPoint.point.pointType == EnumMsg.PointType.虚拟点 || nextPoint.point.pointType == EnumMsg.PointType.整体旋转点)
                        //            {
                        //                if ((int)agv.dialOri != 1) break;
                        //            }
                        //            else
                        //            {
                        //                if ((int)agv.dialOri != 2) break;
                        //            }
                        //            //if (nextPoint.point.pointType == EnumMsg.PointType.虚拟点 && (int)agv.dialOri != 1) break;
                        //            //if (nextPoint.point.pointType != EnumMsg.PointType.虚拟点 && (int)agv.dialOri != 2) break;
                        //        }
                        //        else
                        //        {
                        //            if ((int)agv.dialOri != 1) break;
                        //        }
                        //    }
                        //}

                        #endregion 注释 整体旋转点托盘旋转

                        #region 防止回环申请的管控

                        bool isLoop = false;
                        var agvList = new List<Base_Agv>() { agv };
                        Loop(pathPoint, agvList, ref isLoop);
                        if (isLoop)
                        {
                            //只有一个点的时候才报错
                            if (rcsData.Details.Count == 1)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "回环申请的管控；");
                            }
                            break;
                        }

                        #endregion 防止回环申请的管控

                        #region 特殊区域的流量管控

                        if (point.RegionName.Contains("KeyArea"))
                        {
                            var keyPointList = App.PointList.Where(a => a.RegionName == point.RegionName).ToList();
                            bool isKeyResult = KeyAreaContrl(keyPointList, agv);
                            if (isKeyResult)
                            {
                                if (rcsData.Details.Count == 1)
                                {
                                    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "特殊区域的流量管控；");
                                }
                                break;
                            }
                        }

                        #endregion 特殊区域的流量管控

                        #region 特殊点区域流量管控

                        //if (App.Config_PointList.Find(a => a.strBarcode == point.strBarcode) != null)
                        //{
                        //    List<Config_Point> config_PointList = App.Config_PointList.FindAll(a => a.intNo == App.Config_PointList.Find(b => b.strBarcode == point.strBarcode).intNo && a.areaType == point.AreaType);
                        //    int flag = 0;
                        //    foreach (Config_Point config_Point in config_PointList)
                        //    {
                        //        Point conPoint = App.PointList.Find(a => a.strBarcode == config_Point.strBarcode);
                        //        if (conPoint.lockedAgv != null && conPoint.lockedAgv != agv)// && conPoint.pointType == EnumMsg.PointType.整体旋转点
                        //        {
                        //            if (conPoint.lockedAgv.agvAreaType == "1" && conPoint.lockedAgv.heightState != EnumMsg.HeightState.低位
                        //                   && ((agv.agvAreaType == "1" && agv.heightState != EnumMsg.HeightState.低位) || agv.agvAreaType == "2")) { flag = 99; }
                        //            else if (conPoint.lockedAgv.agvAreaType == "2" && agv.agvAreaType == "1" && agv.heightState != EnumMsg.HeightState.低位)
                        //                flag = 99;
                        //        }
                        //        if (conPoint.lockedAgv != null && conPoint.lockedAgv != agv && conPoint.pointType == EnumMsg.PointType.托盘旋转点)
                        //        { flag = 99; }
                        //    }
                        //    //旋转点是否有车，有车不进入

                        //    if (flag == 99)
                        //    {
                        //        if (motionList.Count == 1)
                        //            agv.strStopReason = ComnMethod.GetStopReason(agv.strStopReason, "特殊点区域的流量管控；");
                        //        break;
                        //    }
                        //}

                        #endregion 特殊点区域流量管控

                        /**
                         * 站台区域避让
                         * 当agv的子任务中点位站台附属点或者下一个点位站台点时判断站台是否存在agv
                         */

                        if (pathPoint.Point.PointType == EnumMsg.PointType.站台附属点
                            && (pathPoint.Barcode == subTask.EndBarcode
                                || (subTask.PathPointList.FirstOrDefault(x => x.SerialNo == pathPoint.SerialNo + 1) is Base_PathPoint nextPath
                                    && App.StationList.Any(x => x.Barcode == nextPath.Barcode))))
                        {
                            var station = App.StationList.Find(x => x.PreBarcode == pathPoint.Barcode);
                            if (station != null &&
                                (App.PointList.Any(x => x.Barcode == station.Barcode && x.LockedAgv != null && x.LockedAgv != agv)
                                || App.AgvList.Any(x => x.Barcode == station.Barcode && x != agv))
                                )
                            {
                                App.ExFile.MessageBusinessErr($"{agv.AgvNo}Motions", $"AGV{agv.AgvNo}在执行任务{agv?.AgvTask?.TaskNo}时，在点{agv?.Barcode}避让站台{station.Name}车辆");
                                break;
                            }


                        }



                    }

                    point.LockedAgv = agv;
                    lockedPoint.Add(point);
                    var rcsDataDetail = GetRcsDataDetail(agv, subTask, pathPoint);

                    if (rcsDataDetail.ActionType != rcsData.ActionType)
                    {
                        if (rcsData.Details.Count <= 0)
                            rcsData.ActionType = rcsDataDetail.ActionType;
                        rcsData.Details.Add(rcsDataDetail);
                        return rcsData;
                    }
                    rcsData.Details.Add(rcsDataDetail);

                    #region 对接等待点的处理
                    if (point.PointType == EnumMsg.PointType.对接等待点)
                    {
                        //如果还有下个路径，就进行处理
                        var nextPath = pathList.Where(x => x.SerialNo > pathPoint.SerialNo).OrderBy(x => x.SerialNo).FirstOrDefault();
                        if (nextPath != null)
                        {
                            var bllResult = BllResult.Success();
                            //如果当前点位是对接等待点，下个点位也是对接等待点，说明要穿越等待点，就要和设备交互
                            if (point.PointType == EnumMsg.PointType.对接等待点 && nextPath.Point.PointType == EnumMsg.PointType.对接等待点)
                            {
                                //如果车没到第一个对接等待点，就不发后面的路径，等车到了再和设备通信。
                                if (agv.Barcode != point.Barcode)
                                {
                                    break;
                                }
                                var equipment = App.EquipmentList.First(t => t.Code.Split('_').Contains(point.Barcode));
                                bllResult = EquipmentExecutor.Instance.RequestOperator(equipment.Name, EnumMsg.RequestType.开快卷门);
                            }
                            //如果当前点位是对接等待点，下个点位也是对接等待点，说明要穿越等待点，就要和设备交互
                            if (point.PointType == EnumMsg.PointType.对接等待点 && nextPath.Point.PointType != EnumMsg.PointType.对接等待点)
                            {
                                //根据配置，如果配置需要关门，才关门
                                if (App.GetKeyValue("CloseDoor") == 1)
                                {
                                    //必须等车到了第二个对接等待点，也就是门后，才能关门
                                    if (agv.Barcode == point.Barcode)
                                    {
                                        var equipment = App.EquipmentList.First(t => t.Code.Split('_').Contains(point.Barcode));
                                        EquipmentExecutor.Instance.RequestOperator(equipment.Name, EnumMsg.RequestType.关快卷门);
                                    }
                                }
                            }
                            //如果AGV和设备交互，失败就之下发到对接等待点，然后返回。
                            if (!bllResult.IsSuccess)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, bllResult.Message);
                                break;
                            }
                        }
                    }
                    #endregion
                }

                if (subTask.TaskStage == EnumMsg.TaskStage.取货中 && subTask.TaskStage == EnumMsg.TaskStage.去取货)
                {
                    rcsData.PickPut = EnumMsg.PickPut.取货;
                }
                else if (subTask.TaskStage == EnumMsg.TaskStage.去放货)
                {
                    rcsData.PickPut = EnumMsg.PickPut.放货;
                }
                //更新AGV锁定点位
                UpdateManage.ClearPointLocked(lockedPoint, agv);

                //要下发的最后一个路径点是拐点且走弧，就不锁定，且不下发给AGV
                if (agv.AgvType == EnumMsg.AgvType.叉车)
                {
                    while (rcsData.Details.Count > 1)
                    {
                        var lastRcsDataDetail = rcsData.Details.Last();
                        if (!lastRcsDataDetail.PathPoint.IsCorner || lastRcsDataDetail.PointType != EnumMsg.PointType.走弧点)
                        {
                            break;
                        }
                        lastRcsDataDetail.PathPoint.Point.LockedAgv = null;
                        rcsData.Details.RemoveAt(rcsData.Details.Count - 1);
                    }
                }


                //如果agv走在两点中间停止，则不再下发上一个点的数据，直接发下一个点数据
                if (agv.AgvState == EnumMsg.AGVState.自动空闲
                    && !agv.IsOnPoint(out _)
                    && agv.IsInMap()
                    && subTask.ActionType.In(EnumMsg.ActionType.普通行走, EnumMsg.ActionType.倒车行走)
                    && rcsData.Details.Count > 1)
                {

                    rcsData.Details[0].Barcode = "0" + rcsData.Details[0].Barcode;
                    rcsData.Details[0].Adjust = 0;
                    rcsData.Details[0].XLength = agv.XLength;
                    rcsData.Details[0].YLength = agv.YLength;
                    App.ExFile.MessageBusinessErr("Motions", $"agv{agv.AgvNo},");
                    App.ExFile.MessageError("Motions", $"agv:{agv.AgvNo}" +
                           $"在执行{agv.AgvTask?.TaskType}任务:{subTask.TaskNo}" +
                           $"的子任务:{subTask?.SubTaskNo}时agv不在点位上,重新调整下发点位坐标{agv.XLength},{agv.YLength},{rcsData.Details[0].Barcode}");
                }
                return rcsData;
            }
            catch (Exception ex)
            {
                throw new Exception($"Motions:原因：{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 获取Motion，重新封装给AGV
        /// </summary>
        /// <param name="sonTask">子任务</param>
        /// <param name="currentPathPoint">待加入的路径点</param>
        /// <returns></returns>
        public static RcsDataDetail GetRcsDataDetail(Base_Agv agv, Base_SubTask sonTask, Base_PathPoint currentPathPoint)
        {
            try
            {

                var rcsDataDetail = new RcsDataDetail();
                rcsDataDetail.ActionType = sonTask.ActionType;

                //调节属性，如果是第一个路径点，就取地标的离开调节属性，如果不是第一个路径点，就取地标的进入调节属性
                var adjust = currentPathPoint.SerialNo == 1 ? currentPathPoint.Point.AdjustOut : currentPathPoint.Point.AdjustIn;
                //雷达
                byte radar = 0;
                //转盘方向
                short? dialAngle = null;
                //速度
                byte speed = 0;
                short? angle = null;
                // ushort agvDirection = 0;
                //根据路径方向，找出对应的车头方向、转盘方向、雷达、速度等
                switch (currentPathPoint.PathDirection)
                {
                    case EnumMsg.Direction.X正方向:
                        radar = currentPathPoint.Point.RadarXPos;
                        //车头角度
                        angle = currentPathPoint.Point.AgvDirectionXPos;
                        dialAngle = currentPathPoint.Point.DialDirectionXPos;
                        speed = currentPathPoint.Point.SpeedXPos;
                        break;
                    case EnumMsg.Direction.Y正方向:
                        radar = currentPathPoint.Point.RadarYPos;
                        angle = currentPathPoint.Point.AgvDirectionYPos;
                        dialAngle = currentPathPoint.Point.DialDirectionYPos;
                        speed = currentPathPoint.Point.SpeedYPos;
                        break;
                    case EnumMsg.Direction.X负方向:
                        radar = currentPathPoint.Point.RadarXNeg;
                        angle = currentPathPoint.Point.AgvDirectionXNeg;

                        dialAngle = currentPathPoint.Point.DialDirectionXNeg;
                        speed = currentPathPoint.Point.SpeedXNeg;
                        break;
                    case EnumMsg.Direction.Y负方向:
                        radar = currentPathPoint.Point.RadarYNeg;
                        angle = currentPathPoint.Point.AgvDirectionYNeg;
                        dialAngle = currentPathPoint.Point.DialDirectionYNeg;
                        speed = currentPathPoint.Point.SpeedYNeg;
                        break;
                }

                if (currentPathPoint.IsCorner && agv.AgvType == EnumMsg.AgvType.叉车)
                {
                    radar = 1;
                }
                if (agv.AgvType != EnumMsg.AgvType.叉车 && dialAngle != null && Math.Abs((short)dialAngle - agv.DialAngle) > 100)
                {
                    //暂时注释
                    rcsDataDetail.DialAngle = (short)ComnMethod.GetAngleDifference((double)dialAngle, (double)agv.DialAngle);
                    rcsDataDetail.ActionType = EnumMsg.ActionType.转盘旋转;
                }

                else if (agv.AgvType == EnumMsg.AgvType.舵轮 && sonTask.ActionType == EnumMsg.ActionType.普通行走)
                {
                    if (angle != null && Math.Abs(agv.Angle - (short)angle) > 100)
                    {
                        rcsDataDetail.AgvAngle = (short)angle;
                        rcsDataDetail.ActionType = EnumMsg.ActionType.旋转;
                        // return rcsDataDetail;
                    }
                    rcsDataDetail.IsLateralDisplacement = true;
                }






                //货物输送方向
                var transportDirection = EnumMsg.Direction.无方向;
                //如果是取货、放货子任务，就要给放货方向赋值
                if (sonTask.ActionType == EnumMsg.ActionType.上层接料 || sonTask.ActionType == EnumMsg.ActionType.下层接料
                    || sonTask.ActionType == EnumMsg.ActionType.上层下料 || sonTask.ActionType == EnumMsg.ActionType.下层下料)
                {
                    //如果是取货，输送方向就是起点站台,如果是放货，输送方向就是终点站台
                    var stationName = (sonTask.ActionType == EnumMsg.ActionType.上层接料 || sonTask.ActionType == EnumMsg.ActionType.下层接料)
                        ? agv.AgvTask?.Initial
                        : agv.AgvTask?.Target;
                    var station = App.StationList.FirstOrDefault(t => t.Name == stationName);
                    if (station != null)
                    {
                        transportDirection = station.StationDirection;
                    }
                }



                //地标编码
                rcsDataDetail.Barcode = currentPathPoint.Point.Barcode;
                //坐标X值
                rcsDataDetail.XLength = currentPathPoint.Point.GetXLength(agv.AgvNo);
                //坐标Y值
                rcsDataDetail.YLength = currentPathPoint.Point.GetYLength(agv.AgvNo);
                //地标属性
                rcsDataDetail.PointType = currentPathPoint.PathPointType;
                //调节属性
                rcsDataDetail.Adjust = adjust;
                //动作
                //rcsDataDetail.ActionType = sonTask.ActionType;
                //顶升高度、充电距离
                rcsDataDetail.Height = sonTask.Height;
                //雷达
                rcsDataDetail.Radar = radar;
                ////车头角度
                //rcsDataDetail.AgvAngle = currentPathPoint.Point.GetAgvAngle(agv.AgvNo);


                ////转盘角度
                //rcsDataDetail.DialAngle = 0;
                //转盘方向

                //货物输送方向
                //1 2是出库   3 4是入库 你那边要分车头方向
                // 2是x正  1是x负
                //入库：3是x负  4是x正

                rcsDataDetail.TransportDirection = transportDirection;
                //行走速度
                rcsDataDetail.Speed = speed;
                //路径点信息
                rcsDataDetail.PathPoint = currentPathPoint;
                return rcsDataDetail;
            }
            catch (Exception ex)
            {
                throw new Exception($"GetMotion:{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 检查是否有回旋死锁，防止三个车环形堵死
        /// </summary>
        /// <param name="nowPoint"></param>
        /// <param name="listAgv"></param>
        /// <param name="isLoop"></param>
        public static void Loop(Base_PathPoint nowPoint, List<Base_Agv> listAgv, ref bool isLoop)
        {
            try
            {
                //找最新的AGV
                var lastAgv = listAgv.Last();
                var nextPoint = ComnMethod.GetNextPathPoint(lastAgv, nowPoint);
                if (nextPoint != null)
                {
                    //获取占用此点的AGV
                    var agv = nextPoint.Point.LockedAgv;
                    //此点被另外的AGV占用了
                    if (agv != null && agv != lastAgv)
                    {
                        listAgv.Add(agv);
                        //如果还没找到第四个点，则继续找
                        if (listAgv.Count == 4)
                        {
                            if (listAgv[0].AgvNo != listAgv[3].AgvNo)
                                isLoop = true;
                            else
                                isLoop = false;

                            return;
                        }
                        Loop(nextPoint, listAgv, ref isLoop);
                    }
                }
            }
            catch (Exception ex)
            {
                throw new Exception($"Loop:{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 区域流量管控
        /// </summary>
        /// <param name="keyPointList"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        public static bool KeyAreaContrl(List<Base_Point> keyPointList, Base_Agv agv)
        {
            try
            {
                if (keyPointList.Exists(x => x.Barcode == agv.Barcode)) return false;
                //提取所有小车的当前点和关键区域所有的点，找相同则为区域内的小车数量
                var agvList = App.AgvList.Select(a => a.Barcode).ToList();
                var keyList = keyPointList.Select(a => a.Barcode).ToList();
                int agvCount = agvList.Intersect(keyList).ToList().Count;
                if (agvCount <= App.KeyAreaNum) return false;
                return true;
            }
            catch (Exception ex)
            {
                throw new Exception($"KeyAreaContrl:{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 互斥区域是否限制进入
        /// </summary>
        /// <param name="mutexPointList">当前点区域集合</param>
        /// <param name="nowPoint">当前点</param>
        /// <param name="agv">申请AGV</param>
        /// <param name="otherAGV">阻挡的AGV</param>
        /// <returns></returns>
        public static bool MutexAreaContrl(List<Base_Point> mutexPointList, Base_PathPoint nowPoint, Base_Agv agv, out Base_Agv? otherAGV)
        {
            try
            {
                otherAGV = null;
                if (mutexPointList.Count == 0)
                {
                    return false;
                }

                //如果点位在管控区，那么也把附属点位加入进来，如果附属点在管控区，那么也把点位加入进来，因为点位和附属点很近
                var points = App.PointList.Where(x => mutexPointList.Exists(y => y.PreBarcode == x.Barcode || y.Barcode == x.PreBarcode));
                mutexPointList.AddRange(points);

                #region 如果子任务的终点有附属点，并附属点和当前点是同一个管控区，就限制进入
                ////当前子任务的终点
                //Base_Point agvEndPoint = agv.AgvTask.SubTaskList.First().EndPoint;
                ////手动模式的任务的agv不进行管控
                ////if (agv.agvTask.strTaskID.Contains("手动")) return false;
                ////子任务终点的附属点
                //Base_Point prePoint = App.PointList.Find(a => a.Barcode == agvEndPoint.PreBarcode);
                ////如果子任务的终点有附属点，并且附属点在管控区内，占用也不是当前AGV申请
                //if (prePoint != null && agvEndPoint.LockedAgv != null && agvEndPoint.LockedAgv != agv &&
                //    mutexPointList.Exists(a => a.RegionName == prePoint.RegionName))
                //{
                //    return true;
                //}
                #endregion

                //管制区的码值
                var mutexBarcodeList = mutexPointList.Select(x => x.Barcode).ToList();

                // 点位不在管控区，就允许进入
                if (!mutexBarcodeList.Contains(nowPoint.Barcode))
                {
                    return false;
                }

                //如果管控区有AGV锁定，且锁定的不是本车，那不许进入
                var otherAgvPoint = mutexPointList.FirstOrDefault(x => x.LockedAgv != null && x.LockedAgv != agv);
                if (otherAgvPoint != null)
                {
                    otherAGV = otherAgvPoint.LockedAgv;
                    return true;
                }

                //找出管制区的所有小车，如果有车，那不许进入
                otherAGV = App.AgvList.FirstOrDefault(x => x.AgvNo != agv.AgvNo && mutexBarcodeList.Contains(x.Barcode));
                if (otherAGV != null)
                {
                    return true;
                }


                #region 注释当前点是停靠点区域判定

                //if (nowPoint.point.pointType == EnumMsg.PointType.停靠附属点)
                //{
                //    PathPoint nextPoint = ComnMethod.GetNextPathPoint(agv, nowPoint);
                //    if (nextPoint.point.pointType == EnumMsg.PointType.停靠点)
                //    {
                //        if (mutexPointList.Exists(a => a.lockedAgv != null && a.lockedAgv != agv)) return true;
                //    }
                //}

                #endregion 注释当前点是停靠点区域判定


                //可以找到该限制区域的所有站台附属点perBarcode
                //找到该限制区域内所有站台点
                //List<string> listBarcode = mutexPointList.Select(a => a.strBarcode).ToList();
                //List<Point> listPoint = App.PointList.FindAll(a => listBarcode.Contains(a.strPreBarcode));

                ////&& listPoint.Exists(a => a.strBarcode == agvEndPoint.strBarcode)
                //PathPoint nextPoint = ComnMethod.GetNextPathPoint(agv, nowPoint);
                //if (listPoint.Exists(a => a.lockedAgv != null && a.lockedAgv != agv
                //&& a.lockedAgv.agvTask!=null && a.pointType != EnumMsg.PointType.停靠点)
                //    && (nextPoint == null || mutexPointList.Exists(a => a.strBarcode == nextPoint.point.strBarcode))) return true;

                return false;
            }
            catch (Exception ex)
            {
                throw new Exception($"MutexAreaContrl:{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 判断当前区域内AGV是否和申请车同方向，如果是可进，不是不可进
        /// </summary>
        /// <param name="mutexPointList"></param>
        /// <param name="agv"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static int StopMutexAreaControl(List<Base_Point> mutexPointList, Base_Agv agv)
        {
            try
            {
                if (mutexPointList.Count == 0)
                {
                    return 0;
                }
                if (agv.AgvTask == null || agv.AgvTask.SubTaskList.Count == 0)
                {
                    return 0;
                }
                var subTask = agv.AgvTask.SubTaskList.First();
                //获取所有限制区域内有任务的其他agv集合
                var agvList = mutexPointList.Where(x => x.LockedAgv != null && x.LockedAgv != agv && x.LockedAgv.AgvTask != null).Select(x => x.LockedAgv).ToList();
                if (agvList.Count == 0)
                {
                    return 0;
                }
                if (agv.AgvTask.EndPoint.RegionName.Contains("Area"))
                {
                    //获取区域内AGV的任务终点和申请车终点在同一区域
                    var agvCount = agvList.Where(x => x.AgvTask.SubTaskList[0].EndPoint.RegionName.Contains("Area")).Count();
                    if (agvCount > 1)
                    {
                        return 1;
                    }
                }
                //判断集合内的是否有任务起点是当前任务终点的AGV
                bool taskAgv = agvList.Exists(x => x.AgvTask.SubTaskList[0].StartPoint == subTask.EndPoint && x.Barcode == subTask.EndPoint.Barcode);
                if (taskAgv)
                {
                    return 1;
                }
                //AGV未行走的路径 与 限制区域路径点的集合
                var agvPathPointList = agv.PathPointList.FindAll(x => mutexPointList.Exists(y => y.Barcode == x.Point.Barcode));
                //申请车有且仅有一个路径点在限制区域内
                if (agvPathPointList.Count == 1 && agvPathPointList.Exists(a => a.PathDirection == 0))
                {
                    return 1;
                }
                //获取已经在限制区域内AGV的路径点
                var pathPointList = agvList.SelectMany(x => x.PathPointList).ToList();
                if (pathPointList.Count == 0)
                {
                    return 0;
                }
                //获取已经在限制区域内AGV的路径点和限制区域集合的交集
                var mutexPathPointList = pathPointList.FindAll(x => mutexPointList.Exists(y => x.Point.Barcode == y.Barcode));
                if (mutexPathPointList.Count() == 0)
                {
                    return 0;
                }
                //申请车在限制区域内的点和已在限制区域内AGV的路径点的交集
                var intersectionPointList = mutexPathPointList.FindAll(x => agv.PathPointList.Exists(y => y.Point.Barcode == x.Point.Barcode));

                //判断两个集合是否存在路径点方向差值等于2的
                var result6 = intersectionPointList.Exists(x => agv.PathPointList.Exists(y => Math.Abs((int)x.PathDirection - (int)y.PathDirection) == 2 && x.Point.Barcode == y.Point.Barcode));
                if (result6)
                {
                    return 1;
                }
                else
                {
                    if (agv.PathPointList.Last().Point.LockedAgv != null)
                    {
                        return 1;
                    }
                    else
                    {
                        return 0;
                    }
                }
            }
            catch (Exception ex)
            {
                throw new Exception($"停靠点判定MutexAreaControl:原因：{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 同向管制区，判断当前区域内AGV是否和申请车同方向，如果是可进，不是不可进
        /// </summary>
        /// <param name="sameDirectionPointList">当前点区域集合</param>
        /// <param name="agv">申请AGV</param>
        /// <param name="otherAGV">阻挡的AGV</param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static bool DirectionAreaControl(List<Base_Point> areaPointList, Base_PathPoint nowPoint, Base_Agv agv, out Base_Agv? otherAGV)
        {
            try
            {
                otherAGV = null;
                if (agv.AgvTask == null || agv.AgvTask.SubTaskList.Count == 0)
                {
                    return false;
                }
                //管制区是空，或者当前点不在管制区，就可以进。
                if (areaPointList.Count == 0 || !areaPointList.Exists(x => x.Barcode == nowPoint.Barcode))
                {
                    return false;
                }
                //如果管制区有其他无任务AGV，就需要让这些车辆移开
                var lockedAgv = areaPointList.Where(x => x.LockedAgv != null && x.LockedAgv != agv && x.LockedAgv.AgvTask == null).Select(x => x.LockedAgv).FirstOrDefault();
                if (lockedAgv != null)
                {
                    otherAGV = lockedAgv;
                    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"要去同向管制区【{nowPoint.Point.RegionName}】地标【{nowPoint.Barcode}】时候，有无任务的AGV【{otherAGV}】需要避让，不能下发路径");
                    return true;
                }
                //获取管制区域内其他有任务的agv集合
                var agvList = areaPointList.Where(x => x.LockedAgv != null && x.LockedAgv != agv && x.LockedAgv.AgvTask != null).Select(x => x.LockedAgv!).ToList();
                if (agvList.Count == 0)
                {
                    return false;
                }
                //如果管制区域有存在任务的车，但是子任务还没执行，那么也不能进入。目的是防止里面的车辆生成一个反向的路径，导致堵死。
                var agvItem = agvList.FirstOrDefault(x => x.AgvTask != null && x.AgvTask.SubTaskList[0].SubTaskState < EnumMsg.SubTaskState.已执行);
                if (agvItem != null)
                {
                    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"要去同向管制区【{nowPoint.Point.RegionName}】地标【{nowPoint.Barcode}】时候，有任务AGV【{agvItem.AgvNo}】未开始执行，不能下发路径");
                    return true;
                }
                //限行区内的其他车路径，如果包含当前车要去的点，那就要等待。因为其他车是异向的，那么会堵死，同向的还没离开也会撞，所以都要等。
                agvItem = agvList.FirstOrDefault(x => x.PathPointList.Exists(y => y.Barcode == nowPoint.Barcode));
                if (agvItem != null)
                {
                    agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"要去同向管制区【{nowPoint.Point.RegionName}】地标【{nowPoint.Barcode}】时候，有AGV【{agvItem.AgvNo}】占用了路径，不能下发路径");
                    return true;
                }
                return false;
            }
            catch (Exception ex)
            {
                throw new Exception($"停靠点判定MutexAreaControl:原因：{ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 判断指定的点位如果去了，是否会撞车
        /// </summary>
        /// <param name="pathPoint">要判断的路径点</param>
        /// <param name="agv">路径点中的AGV</param>
        /// <param name="pathDirection">从什么方向进入当前点位</param>
        /// <param name="otherAGV">需要避让的AGV</param>
        /// <returns></returns>
        public static bool CheckCrash(Base_PathPoint pathPoint, Base_Agv agv, EnumMsg.Direction pathDirection, out Base_Agv? otherAGV)
        {
            try
            {
                otherAGV = null;
                if (agv.AgvTask == null || agv.AgvTask.SubTaskList.Count == 0)
                {
                    return false;
                }
                //第一条子任务，也就是当前子任务
                var subTask = agv.AgvTask.SubTaskList.First();
                //雷达需要的距离
                var radarDistance = 210;
                //默认前方需要的距离：预留4个车头长度。1是为了防止前车后退1个库位，2是为了两个车都要去拐点，相互卡死。
                var frontDistance = agv.HeadLength * 4;
                //默认侧面需要的距离：车头长度 + 车宽的一半 + 雷达距离
                var sideDistance = agv.HeadLength + agv.Width / 2 + radarDistance;
                var otherAGVPointList = new List<Base_Point>();
                #region 寻找4个方向的站台，获取最大IntX、最小IntX、最大IntY、最小IntY，用户限定搜索范围，因为站台后面的车，是不会影响本车的。
                int maxIntX = 9999;
                int minIntX = 0;
                int maxIntY = 9999;
                int minIntY = 0;
                var stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.XPosPoint != null)
                {
                    stationPoint = stationPoint.XPosPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        maxIntX = stationPoint.IntX;
                        break;
                    }
                }
                stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.XNegPoint != null)
                {
                    stationPoint = stationPoint.XNegPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        minIntX = stationPoint.IntX;
                        break;
                    }
                }
                stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.YPosPoint != null)
                {
                    stationPoint = stationPoint.YPosPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        maxIntY = stationPoint.IntY;
                        break;
                    }
                }
                stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.YNegPoint != null)
                {
                    stationPoint = stationPoint.YNegPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        minIntY = stationPoint.IntY;
                        break;
                    }
                }
                #endregion
                // 沿着路径方向找有车锁定的点位，直到站台或是不会撞的距离为止。注意：金利隆地图X正是坐标Y正，地图X负是坐标Y负，地图Y正是坐标X负，地图Y负是坐标X正
                switch (pathDirection)
                {
                    case EnumMsg.Direction.X正方向:
                        otherAGVPointList = App.PointList.Where(t => t.AreaType == pathPoint.Point.AreaType && t.GetArea() == pathPoint.Point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                                  && t.IntX >= pathPoint.Point.IntX && t.IntX <= maxIntX && t.IntY >= minIntY && t.IntY <= maxIntY
                                                                  && t.YLength <= pathPoint.Point.YLength + frontDistance
                                                                  && t.XLength >= pathPoint.Point.XLength - sideDistance && t.XLength <= pathPoint.Point.XLength + sideDistance).ToList();
                        break;
                    case EnumMsg.Direction.X负方向:
                        otherAGVPointList = App.PointList.Where(t => t.AreaType == pathPoint.Point.AreaType && t.GetArea() == pathPoint.Point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                                  && t.IntX <= pathPoint.Point.IntX && t.IntX >= minIntX && t.IntY >= minIntY && t.IntY <= maxIntY
                                                                  && t.YLength >= pathPoint.Point.YLength - frontDistance
                                                                  && t.XLength >= pathPoint.Point.XLength - sideDistance && t.XLength <= pathPoint.Point.XLength + sideDistance).ToList();
                        break;
                    case EnumMsg.Direction.Y正方向:
                        otherAGVPointList = App.PointList.Where(t => t.AreaType == pathPoint.Point.AreaType && t.GetArea() == pathPoint.Point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                                  && t.IntY >= pathPoint.Point.IntY && t.IntY <= maxIntY && t.IntX >= minIntX && t.IntX <= maxIntX
                                                                  && t.XLength >= pathPoint.Point.XLength - frontDistance
                                                                  && t.YLength >= pathPoint.Point.YLength - sideDistance && t.YLength <= pathPoint.Point.YLength + sideDistance).ToList();
                        break;
                    case EnumMsg.Direction.Y负方向:
                        otherAGVPointList = App.PointList.Where(t => t.AreaType == pathPoint.Point.AreaType && t.GetArea() == pathPoint.Point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                                  && t.IntY <= pathPoint.Point.IntY && t.IntY >= minIntY && t.IntX >= minIntX && t.IntX <= maxIntX
                                                                  && t.XLength <= pathPoint.Point.XLength + frontDistance
                                                                  && t.YLength >= pathPoint.Point.YLength - sideDistance && t.YLength <= pathPoint.Point.YLength + sideDistance).ToList();
                        break;
                }
                //如果路径方向有车，就计算够不够远，两个车会不会撞
                foreach (var otherAGVPoint in otherAGVPointList)
                {
                    if (otherAGVPoint.LockedAgv == null || otherAGVPoint.PointType == EnumMsg.PointType.充电点)
                    {
                        continue;
                    }
                    //金利隆地图X正是坐标Y正，地图X负是坐标Y负，地图Y正是坐标X负，地图Y负是坐标X正
                    var yDistance = Math.Abs(otherAGVPoint.YLength - pathPoint.Point.YLength);
                    //金利隆地图X正是坐标Y正，地图X负是坐标Y负，地图Y正是坐标X负，地图Y负是坐标X正
                    var xDistance = Math.Abs(otherAGVPoint.XLength - pathPoint.Point.XLength);
                    //车头方向
                    var otherAgvDirectionList = new List<EnumMsg.Direction>();
                    //如果不是AGV当前点（AGV会按照路径方向走），或者AGV是忙碌（可能会原地旋转），就按照路径点的方向计算。
                    if (otherAGVPoint.Barcode != otherAGVPoint.LockedAgv.Barcode || otherAGVPoint.LockedAgv.AgvState == EnumMsg.AGVState.自动忙碌)
                    {
                        //如果点位在路径中，就计算进入点位和离开点位的防撞。如果不在路径中，就按照点位所有能通行的方向计算。
                        var otherAGVPathPoint = otherAGVPoint.LockedAgv.PathPointList.FirstOrDefault(x => x.Barcode == otherAGVPoint.Barcode);
                        if (otherAGVPathPoint != null)
                        {
                            //进入点位的方向，走弧进入的话 ，占用其实更少。
                            otherAgvDirectionList.Add(otherAGVPathPoint.PreviousPathDirection);
                            //要把离开的方向也算进去，走弧离开的话，车尾方向只需要多占用10厘米，所以反方向可以忽略。
                            if (!otherAgvDirectionList.Contains(otherAGVPathPoint.PathDirection))
                            {
                                otherAgvDirectionList.Add(otherAGVPathPoint.PathDirection);
                            }
                        }
                        else
                        {
                            if (otherAGVPoint.IsXPos) otherAgvDirectionList.Add(EnumMsg.Direction.X正方向);
                            if (otherAGVPoint.IsXNeg) otherAgvDirectionList.Add(EnumMsg.Direction.X负方向);
                            if (otherAGVPoint.IsYPos) otherAgvDirectionList.Add(EnumMsg.Direction.Y正方向);
                            if (otherAGVPoint.IsYNeg) otherAgvDirectionList.Add(EnumMsg.Direction.Y正方向);
                        }
                    }
                    //如果AGV在点位上，还要按照实际的车头方向来计算防撞。
                    if (otherAGVPoint.Barcode == otherAGVPoint.LockedAgv.Barcode && !otherAgvDirectionList.Contains(otherAGVPoint.LockedAgv.AgvDirectionByMap))
                    {
                        otherAgvDirectionList.Add(otherAGVPoint.LockedAgv.AgvDirectionByMap);
                    }
                    //根据当前和前面车的16个方向，判断距离是否会撞击。注意：金利隆地图X正是坐标Y正，地图X负是坐标Y负，地图Y正是坐标X负，地图Y负是坐标X正
                    foreach (var otherAgvDirection in otherAgvDirectionList)
                    {
                        if ((pathDirection == EnumMsg.Direction.X正方向 && otherAgvDirection == EnumMsg.Direction.X正方向) ||
                            (pathDirection == EnumMsg.Direction.X负方向 && otherAgvDirection == EnumMsg.Direction.X负方向))
                        {
                            if (subTask.ActionType == EnumMsg.ActionType.普通行走 && pathPoint.IsCorner)
                            {
                                frontDistance = agv.HeadLength + agv.TailLength;
                            }
                            if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                            {
                                frontDistance = agv.TailLength + agv.TailLength;
                            }
                            if (xDistance <= agv.Width && yDistance <= frontDistance)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"前往【{pathDirection}】地标【{pathPoint.Barcode}】会和地标【{otherAGVPoint.Barcode}】的车【{otherAGVPoint.LockedAgv.AgvNo}】【{otherAgvDirection}】相撞！");
                                otherAGV = otherAGVPoint.LockedAgv;
                                return true;
                            }
                        }

                        if ((pathDirection == EnumMsg.Direction.X正方向 && otherAgvDirection == EnumMsg.Direction.X负方向) ||
                            (pathDirection == EnumMsg.Direction.X负方向 && otherAgvDirection == EnumMsg.Direction.X正方向))
                        {
                            if (subTask.ActionType == EnumMsg.ActionType.普通行走 && pathPoint.IsCorner)
                            {
                                frontDistance = agv.HeadLength + agv.HeadLength;
                            }
                            if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                            {
                                frontDistance = agv.TailLength + agv.HeadLength;
                            }
                            if (xDistance <= agv.Width && yDistance <= frontDistance)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"前往【{pathDirection}】地标【{pathPoint.Barcode}】会和地标【{otherAGVPoint.Barcode}】的车【{otherAGVPoint.LockedAgv.AgvNo}】【{otherAgvDirection}】相撞！");
                                otherAGV = otherAGVPoint.LockedAgv;
                                return true;
                            }
                        }

                        if ((pathDirection == EnumMsg.Direction.X正方向 && otherAgvDirection == EnumMsg.Direction.Y正方向) ||
                            (pathDirection == EnumMsg.Direction.X负方向 && otherAgvDirection == EnumMsg.Direction.Y负方向) ||
                            (pathDirection == EnumMsg.Direction.X正方向 && otherAgvDirection == EnumMsg.Direction.Y负方向) ||
                            (pathDirection == EnumMsg.Direction.X负方向 && otherAgvDirection == EnumMsg.Direction.Y正方向))
                        {
                            if (subTask.ActionType == EnumMsg.ActionType.普通行走 && pathPoint.IsCorner)
                            {
                                frontDistance = agv.HeadLength + agv.Width / 2;
                            }
                            if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                            {
                                frontDistance = agv.TailLength + agv.Width / 2;
                            }
                            if ((otherAgvDirection == EnumMsg.Direction.Y正方向 && otherAGVPoint.IntY >= pathPoint.Point.IntY) ||
                                (otherAgvDirection == EnumMsg.Direction.Y负方向 && otherAGVPoint.IntY <= pathPoint.Point.IntY))
                            {
                                sideDistance = agv.TailLength + agv.Width / 2 + radarDistance;
                            }
                            if (xDistance <= sideDistance && yDistance <= frontDistance)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"前往【{pathDirection}】地标【{pathPoint.Barcode}】会和地标【{otherAGVPoint.Barcode}】的车【{otherAGVPoint.LockedAgv.AgvNo}】【{otherAgvDirection}】相撞！");
                                otherAGV = otherAGVPoint.LockedAgv;
                                return true;
                            }
                        }

                        if ((pathDirection == EnumMsg.Direction.Y正方向 && otherAgvDirection == EnumMsg.Direction.Y正方向) ||
                            (pathDirection == EnumMsg.Direction.Y负方向 && otherAgvDirection == EnumMsg.Direction.Y负方向))
                        {
                            if (subTask.ActionType == EnumMsg.ActionType.普通行走 && pathPoint.IsCorner)
                            {
                                frontDistance = agv.HeadLength + agv.TailLength;
                            }
                            if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                            {
                                frontDistance = agv.TailLength + agv.TailLength;
                            }
                            if (yDistance <= agv.Width && xDistance <= frontDistance)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"前往【{pathDirection}】地标【{pathPoint.Barcode}】会和地标【{otherAGVPoint.Barcode}】的车【{otherAGVPoint.LockedAgv.AgvNo}】【{otherAgvDirection}】相撞！");
                                otherAGV = otherAGVPoint.LockedAgv;
                                return true;
                            }
                        }

                        if ((pathDirection == EnumMsg.Direction.Y正方向 && otherAgvDirection == EnumMsg.Direction.Y负方向) ||
                            (pathDirection == EnumMsg.Direction.Y负方向 && otherAgvDirection == EnumMsg.Direction.Y正方向))
                        {
                            if (subTask.ActionType == EnumMsg.ActionType.普通行走 && pathPoint.IsCorner)
                            {
                                frontDistance = agv.HeadLength + agv.HeadLength;
                            }
                            if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                            {
                                frontDistance = agv.TailLength + agv.HeadLength;
                            }
                            if (yDistance <= agv.Width && xDistance <= frontDistance)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"前往【{pathDirection}】地标【{pathPoint.Barcode}】会和地标【{otherAGVPoint.Barcode}】的车【{otherAGVPoint.LockedAgv.AgvNo}】【{otherAgvDirection}】相撞！");
                                otherAGV = otherAGVPoint.LockedAgv;
                                return true;
                            }
                        }

                        if ((pathDirection == EnumMsg.Direction.Y正方向 && otherAgvDirection == EnumMsg.Direction.X正方向) ||
                            (pathDirection == EnumMsg.Direction.Y负方向 && otherAgvDirection == EnumMsg.Direction.X负方向) ||
                            (pathDirection == EnumMsg.Direction.Y正方向 && otherAgvDirection == EnumMsg.Direction.X负方向) ||
                            (pathDirection == EnumMsg.Direction.Y负方向 && otherAgvDirection == EnumMsg.Direction.X正方向))
                        {
                            if (subTask.ActionType == EnumMsg.ActionType.普通行走 && pathPoint.IsCorner)
                            {
                                frontDistance = agv.HeadLength + agv.Width / 2;
                            }
                            if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                            {
                                frontDistance = agv.TailLength + agv.Width / 2;
                            }
                            if ((otherAgvDirection == EnumMsg.Direction.X正方向 && otherAGVPoint.IntX >= pathPoint.Point.IntX) ||
                                (otherAgvDirection == EnumMsg.Direction.X负方向 && otherAGVPoint.IntX <= pathPoint.Point.IntX))
                            {
                                sideDistance = agv.TailLength + agv.Width / 2 + radarDistance;
                            }
                            if (yDistance <= sideDistance && xDistance <= frontDistance)
                            {
                                agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"前往【{pathDirection}】地标【{pathPoint.Barcode}】会和地标【{otherAGVPoint.Barcode}】的车【{otherAGVPoint.LockedAgv.AgvNo}】【{otherAgvDirection}】相撞！");
                                otherAGV = otherAGVPoint.LockedAgv;
                                return true;
                            }
                        }
                    }
                }
                return false;
            }
            catch (Exception ex)
            {
                throw new Exception($"CheckCrash:: {ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 判断点位如果去了，是否会撞车
        /// </summary>
        /// <param name="pathPoint">要判断的路径点</param>
        /// <param name="agv">路径点中的AGV</param>
        /// <param name="pathDirection">从什么方向进入当前点位</param>
        /// <param name="otherAGV">需要避让的AGV</param>
        /// <returns></returns>
        public static bool CheckCrash_Old(Base_PathPoint pathPoint, Base_Agv agv, EnumMsg.Direction pathDirection, out Base_Agv? otherAGV)
        {
            try
            {
                otherAGV = null;
                if (agv.AgvTask == null || agv.AgvTask.SubTaskList.Count == 0)
                {
                    return false;
                }
                var point = pathPoint.Point;
                var subTask = agv.AgvTask.SubTaskList.First();
                #region 寻找4个方向的站台，获取最大IntX、最小IntX、最大IntY、最小IntY，用户限定搜索范围，因为站台后面的车，是不会影响本车的。
                int maxIntX = 9999;
                int minIntX = 0;
                int maxIntY = 9999;
                int minIntY = 0;
                var stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.XPosPoint != null)
                {
                    stationPoint = stationPoint.XPosPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        maxIntX = stationPoint.IntX;
                        break;
                    }
                }
                stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.XNegPoint != null)
                {
                    stationPoint = stationPoint.XNegPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        minIntX = stationPoint.IntX;
                        break;
                    }
                }
                stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.YPosPoint != null)
                {
                    stationPoint = stationPoint.YPosPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        maxIntY = stationPoint.IntY;
                        break;
                    }
                }
                stationPoint = pathPoint.Point;
                while (stationPoint != null && stationPoint.YNegPoint != null)
                {
                    stationPoint = stationPoint.YNegPoint;
                    if (stationPoint.PointType == EnumMsg.PointType.站台点)
                    {
                        minIntY = stationPoint.IntY;
                        break;
                    }
                }
                #endregion
                var agvPointList = new List<Base_Point>();
                // 沿着路径方向找有车锁定的点位，直到站台为止
                switch (pathDirection)
                {
                    case EnumMsg.Direction.X正方向:
                        agvPointList = App.PointList.Where(t => t.GetArea() == point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                             && t.IntX >= point.IntX && t.IntX <= maxIntX && t.IntY >= minIntY && t.IntY <= maxIntY).ToList();
                        break;
                    case EnumMsg.Direction.X负方向:
                        agvPointList = App.PointList.Where(t => t.GetArea() == point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                             && t.IntX <= point.IntX && t.IntX >= minIntX && t.IntY >= minIntY && t.IntY <= maxIntY).ToList();
                        break;
                    case EnumMsg.Direction.Y正方向:
                        agvPointList = App.PointList.Where(t => t.GetArea() == point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                             && t.IntY >= point.IntY && t.IntY <= maxIntY && t.IntX >= minIntX && t.IntX <= maxIntX).ToList();
                        break;
                    case EnumMsg.Direction.Y负方向:
                        agvPointList = App.PointList.Where(t => t.GetArea() == point.GetArea() && t.LockedAgv != null && t.LockedAgv != agv
                                                             && t.IntY <= point.IntY && t.IntY >= minIntY && t.IntX >= minIntX && t.IntX <= maxIntX).ToList();
                        break;
                }
                //如果路径方向有车，就计算够不够远，两个车会不会撞
                foreach (var agvPoint in agvPointList)
                {
                    if (agvPoint.LockedAgv == null || agvPoint.PointType == EnumMsg.PointType.充电点)
                    {
                        continue;
                    }
                    var yDistance = Math.Abs(agvPoint.YLength - point.YLength);
                    var xDistance = Math.Abs(agvPoint.XLength - point.XLength);

                    //默认距离，预留2个车头长度
                    var minDistance = agv.HeadLength * 2;
                    //倒车行走需要最少留1个车头的距离 + 车尾距离
                    if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
                    {
                        minDistance = agv.HeadLength + agv.TailLength;
                    }
                    else
                    {
                        //如果前车不在管制区、且不是拐点，要留4个车头的长度。一是为了防止前车后退1个库位，二是为了两个车都要去拐点，相互卡死。
                        if (string.IsNullOrEmpty(agvPoint.RegionName) && !pathPoint.IsCorner)
                        {
                            minDistance = agv.HeadLength * 4;
                        }
                    }
                    //增加300毫米的雷达距离
                    minDistance = minDistance + 300;
                    if (pathDirection == EnumMsg.Direction.X正方向 || pathDirection == EnumMsg.Direction.X负方向)
                    {
                        //计算如果前车是Y方向，本车是X方向，计算是否会撞
                        if (agvPoint.IsYPos || agvPoint.IsYNeg)
                        {
                            if (xDistance <= agv.HeadLength + agv.Width / 2 && yDistance <= agv.HeadLength + agv.Width / 2)
                            {
                                otherAGV = agvPoint.LockedAgv;
                                return true;
                            }
                        }
                        //计算如果前车是X方向，本车是X方向，计算是否会撞
                        if (agvPoint.IsXPos || agvPoint.IsXNeg)
                        {
                            //计算车头都是X方向，是否会撞
                            if (xDistance <= agv.Width && yDistance <= minDistance)
                            {
                                otherAGV = agvPoint.LockedAgv;
                                return true;
                            }
                        }
                    }
                    if (pathDirection == EnumMsg.Direction.Y正方向 || pathDirection == EnumMsg.Direction.Y负方向)
                    {
                        //如果点位可以沿着X方向行走，那车可能会横过来
                        if (agvPoint.IsXPos || agvPoint.IsXNeg)
                        {
                            // 计算如果前车是X方向，本车是Y方向，计算是否会撞
                            if (yDistance <= agv.HeadLength + agv.Width / 2 && xDistance <= agv.HeadLength + agv.Width / 2)
                            {
                                otherAGV = agvPoint.LockedAgv;
                                return true;
                            }
                        }
                        //计算如果前车是Y方向，本车是Y方向，计算是否会撞
                        if (agvPoint.IsYPos || agvPoint.IsYNeg)
                        {
                            //计算车头都是Y方向，是否会撞
                            if (yDistance <= agv.Width && xDistance <= minDistance)
                            {
                                otherAGV = agvPoint.LockedAgv;
                                return true;
                            }
                        }
                    }
                }
                return false;
            }
            catch (Exception ex)
            {
                throw new Exception($"CheckCrash:: {ex.Message}，\n 堆栈：{ex.StackTrace}");
            }
        }

        /// <summary>
        /// 获取站台附属点的前置点
        /// </summary>
        /// <param name="stationPoint">站台点</param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Base_Point? GetStatioPoint(Base_Point stationPoint, EnumMsg.Direction direction)
        {
            Base_Point point = stationPoint;

            return point;
        }

        /// <summary>
        /// 生成回家避让任务
        /// </summary>
        /// <param name="agvTask">被阻挡的任务</param>
        /// <param name="targetAGV">需要执行避让的AGV</param>
        /// <exception cref="Exception"></exception>
        public static void AvoidTask(Base_Agv currentAGV, Base_Agv? targetAGV)
        {
            try
            {
                var msg = string.Empty;
                if (targetAGV == null)
                {
                    msg = $"需要避让的AGV为空，无法避让！";
                    currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                    return;
                }
                if (string.IsNullOrEmpty(targetAGV.Barcode))
                {
                    msg = $"需要避让的AGV【{targetAGV.AgvNo}】当前点位为空，无法避让！";
                    currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                    return;
                }
                if ((DateTime.Now - targetAGV.FreeTime).TotalSeconds < 3)
                {
                    msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】要空闲超过3秒，才会生成避让！";
                    currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                    return;
                }
                var task = App.TaskList.FirstOrDefault(t => t.TaskAgvNo == targetAGV.AgvNo);
                if (task != null)
                {
                    if (task.TaskType == EnumMsg.TaskType.回家.ToString())
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】已经有【回家】任务，等待前车执行！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    if (task.TaskType == EnumMsg.TaskType.取消充电.ToString())
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】已经有【取消充电】任务，等待前车执行！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    if (task.TaskType == EnumMsg.TaskType.充电.ToString())
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】已经有【充电】任务，等待前车执行！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                        //var subTask = task.SubTaskList.FirstOrDefault();
                        //if (subTask != null && subTask.ActionType != EnumMsg.ActionType.倒车行走 && subTask.ActionType != EnumMsg.ActionType.充电)
                        //{
                        //    var result = TaskManage.TaskDelete(task);
                        //    currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, result.Message);
                        //    return;
                        //}
                    }
                    msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】存在【{task.TaskType}】任务【{task.TaskNo}】，不能生成避让任务，请等待前车执行！";
                    currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                    return;
                }

                //如果车在充电中，且AGV没有取消充电的任务，就生成取消充电的任务
                if (targetAGV.AgvState == EnumMsg.AGVState.充电中)
                {
                    if (App.TaskList.Any(t => t.TaskAgvNo == targetAGV.AgvNo && t.TaskType == EnumMsg.TaskType.取消充电.ToString()))
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】充电中，已生成取消充电任务，等待执行！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    var result = TaskManage.TaskBuild(EnumMsg.TaskType.取消充电.ToString(),
                                                      EnumMsg.TaskGroup.RCS单步任务.ToString(),
                                                      "1", 1, targetAGV.AgvNo, targetAGV.Barcode, targetAGV.Barcode, "", "", "");
                    if (result.IsSuccess)
                    {
                        App.ExFile.MessageLog("TaskBuild", $"因为需要避让，生成AGV【{targetAGV.AgvNo}】的【取消充电】任务！");
                    }
                    else
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】生成【取消充电】任务失败，原因：{result.Message}！";
                        App.ExFile.MessageBusinessErr("TaskBuild", msg);
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    return;
                }

                // 如果车是自动空闲，且没有手动行走的任务，就生成行走任务来避让
                if (targetAGV.AgvState == EnumMsg.AGVState.自动空闲)
                {
                    //var stopPoint = ComnMethod.GetMinHomePoint(targetAGV);
                    var stopPoint = ComnMethod.GetMinStationPoint(targetAGV);
                    if (stopPoint == null)
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】获取不到回家位置，无法生成回家任务！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    if (stopPoint.Barcode == targetAGV.Barcode)
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】就在回家点，请等待！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    if (App.TaskList.Any(t => t.TaskAgvNo == targetAGV.AgvNo && t.TaskType == EnumMsg.TaskType.回家.ToString()))
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】已经生成【回家】任务，等待前面的车离开！";
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }

                    var result = TaskManage.TaskBuild(EnumMsg.TaskType.回家.ToString(),
                                                      EnumMsg.TaskGroup.RCS单步任务.ToString(), "1", 1, targetAGV.AgvNo, targetAGV.Barcode, stopPoint.Barcode, "", "", "");
                    if (result.IsSuccess)
                    {
                        App.ExFile.MessageLog("TaskBuild", $"因为需要避让，生成AGV【{targetAGV.AgvNo}】的【回家】任务！");
                    }
                    else
                    {
                        msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】生成【回家】任务失败，原因：{result.Message}！";
                        App.ExFile.MessageBusinessErr("TaskBuild", msg);
                        currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                        return;
                    }
                    return;
                }

                msg = $"需要避让点位【{targetAGV.Barcode}】的AGV【{targetAGV.AgvNo}】不是【自动空闲】或是【充电中】，不能生成避让任务！";
                currentAGV.StopReason = ComnMethod.GetStopReason(currentAGV.StopReason, msg);
                return;
            }
            catch (Exception ex)
            {
                throw new Exception($"AvoidTask:: {ex.Message}，\n 堆栈：{ex.StackTrace}");
            }

        }

    }
}