using AngleSharp.Dom;
|
using Furion.Logging;
|
using OfficeOpenXml.FormulaParsing.Excel.Functions.Logical;
|
using System;
|
using System.Collections.Generic;
|
using System.Linq;
|
using System.Text;
|
using System.Threading.Tasks;
|
using WCS.Application.Entity;
|
|
namespace WCS.Application.Util;
|
|
public static class FourWayCarUtil
|
{
|
private static readonly ISqlSugarClient _db = SqlSugarSetup.ITenant.GetConnectionScope(SqlSugarConst.MainConfigId);
|
|
/// <summary>
|
///
|
/// </summary>
|
/// <param name="list">集合</param>
|
/// <param name="moveType">移动类型 0:移动 1:移货 </param>
|
/// <returns></returns>
|
public static List<CarModel> GetCarPathUp(List<CarModel> list,int moveType)
|
{
|
for (int i = 0; i < list.Count; i++)
|
{
|
if (i > 0 && i < list.Count - 1)
|
{
|
if (list[i].X == list[i + 1].X)
|
{
|
list[i].NodeCom = 2;
|
}
|
else if (list[i].Y == list[i + 1].Y)
|
{
|
list[i].NodeCom = 3;
|
}
|
}
|
if (i== 0)
|
{
|
list[i].IsSendPlc = true;
|
if (moveType == 0)
|
{
|
if (list[i + 1] == null)
|
{
|
continue;
|
}
|
|
}
|
else
|
{
|
list[i].NodeCom = 1;
|
}
|
}
|
if (i == list.Count-1)
|
{
|
list[i].IsSendPlc = true;
|
if (moveType == 0)
|
{
|
list[i].NodeCom = list[i + 1].NodeCom;
|
}
|
else
|
{
|
list[i].NodeCom = 4;
|
}
|
}
|
}
|
return list;
|
}
|
|
/// <summary>
|
/// 获取小车路径
|
/// </summary>
|
/// <param name="startLocation">起始位置</param>
|
/// <param name="endLocation">目标位置</param>
|
/// <param name="isLoad">是否载货0：未载货 1：已载货</param>
|
/// <returns></returns>
|
public static List<CarModel> GetCarPath(string startLocation, string endLocation, string isLoad = "0")
|
{
|
if (string.IsNullOrEmpty(startLocation) || string.IsNullOrEmpty(endLocation))
|
{
|
return null;
|
}
|
|
// 起始位置
|
CarModel start = new CarModel()
|
{
|
X = int.Parse(startLocation.Substring(0, 2)),
|
Y = int.Parse(startLocation.Substring(2, 2)),
|
Z = int.Parse(startLocation.Substring(4, 2)),
|
NodeCom = 0
|
};
|
|
// 目标位置
|
CarModel end = new CarModel()
|
{
|
X = int.Parse(endLocation.Substring(0, 2)),
|
Y = int.Parse(endLocation.Substring(2, 2)),
|
Z = int.Parse(endLocation.Substring(4, 2)),
|
NodeCom = 0
|
};
|
|
// 获取储位表信息存储到集合里
|
var layer = int.Parse(startLocation.Substring(4, 2));
|
|
// 获取当前层储位信息
|
var locationModels = _db.Queryable<WcsStorageLocat>()
|
.Where(m => m.WareHouseNo == "W01" && m.Layer == layer && m.IsDelete == false)
|
.ToList();
|
|
#region 使用算法计算小车路径
|
try
|
{
|
// 定义开发列表存储路径节点
|
var openSet = new SortedSet<(int fscore, CarModel pos)>();
|
// 定义关闭节点字典
|
var closeSet = new Dictionary<CarModel, CarModel>();
|
// 定义上一位置与目标位置字典
|
var cameFrom = new Dictionary<CarModel, CarModel>();
|
// 定义上一位置与目标位置的实际距离字典
|
var gScore = new Dictionary<CarModel, int>();
|
// 定义上一位置与目标位置的预估距离字典
|
var fScore = new Dictionary<CarModel, int>();
|
|
// 存储最优距离，及起始节点
|
openSet.Add((Heuristic(start, end), start));
|
gScore[start] = 0;
|
fScore[start] = Heuristic(start, end);
|
|
// 循环查找路径
|
while (openSet.Count > 0)
|
{
|
var current = openSet.Min.pos;
|
openSet.Remove(openSet.Min);
|
|
if (current.Equals(end))
|
{
|
Log.Error(ReconstructPath(cameFrom, current).ToString());
|
return ReconstructPath(cameFrom, current);
|
}
|
|
// 存储小车可运行的方向
|
var validDirections = new List<CarModel>();
|
var currentLocation = locationModels.FirstOrDefault(m => m.Row == current.X && m.Column == current.Y);
|
if (currentLocation.Make == "0")
|
{
|
// 主通道
|
validDirections.Add(new CarModel() { X = 1, Y = 0 }); // 右
|
validDirections.Add(new CarModel() { X = -1, Y = 0 }); // 左
|
validDirections.Add(new CarModel() { X = 0, Y = 1 }); // 下
|
validDirections.Add(new CarModel() { X = 0, Y = -1 }); // 上
|
}
|
|
if (currentLocation.Make == "1")
|
{
|
// 子通道
|
// 先拆分出口
|
var outNode = currentLocation.AisleOne;
|
if (string.IsNullOrEmpty(outNode))
|
{
|
throw new Exception("当前位置没有维护出口！");
|
}
|
int outX = int.Parse(outNode.Substring(0, 2));
|
int outY = int.Parse(outNode.Substring(2, 2));
|
if (current.X == outX)
|
{
|
validDirections.Add(new CarModel() { X = 0, Y = 1 }); // 下
|
validDirections.Add(new CarModel() { X = 0, Y = -1 }); // 上
|
|
}
|
else
|
{
|
validDirections.Add(new CarModel() { X = 1, Y = 0 }); // 右
|
validDirections.Add(new CarModel() { X = -1, Y = 0 }); // 左
|
}
|
}
|
|
// 循环连接节点。
|
bool isNextNode = false;
|
foreach (var dir in validDirections)
|
{
|
CarModel neighbor = new CarModel() { X = current.X + dir.X, Y = current.Y + dir.Y, Z = layer };
|
|
// 验证下一节点位置是否可通行并且判断是否被其他小车占用
|
|
|
// 判断下一节点是否关闭
|
if (closeSet.ContainsKey(neighbor))
|
{
|
closeSet[neighbor] = neighbor;
|
}
|
|
// 当前节点
|
var currentModel = locationModels.FirstOrDefault(it => it.Row == current.X && it.Column == current.Y);
|
// 下一节点
|
var locationModel = locationModels.FirstOrDefault(it => it.Row == neighbor.X && it.Column == neighbor.Y);
|
|
// 不存在此位置信息
|
if (locationModel == null)
|
{
|
closeSet[neighbor] = neighbor;
|
continue;
|
}
|
// 储位状态为损坏不可通行 储位状态为屏蔽为可通行不可存储托盘
|
if (locationModel.Flag == "2")
|
{
|
closeSet[neighbor] = neighbor;
|
continue;
|
}
|
// 判断下一节点上是否有托盘
|
if (!string.IsNullOrEmpty(locationModel.PalletNo))
|
{
|
// 判断小车是否载托盘盘
|
if (isLoad == "1")
|
{
|
closeSet[neighbor] = neighbor;
|
// 小车上载托盘不可通行跳过
|
continue;
|
}
|
}
|
// 优化项，验证下一节点是否被别的小车占用 liudl：在此添加优化代码
|
|
// 更新实际距离
|
int tentativeGScore = gScore[current] + 1;
|
// 判断位置是否已包含在路径内 且 是否更近节点
|
if (!gScore.ContainsKey(neighbor) || tentativeGScore < gScore[neighbor])
|
{
|
neighbor.IsSendPlc = false;
|
|
//if (current.X == neighbor.X)
|
//{
|
// current.NodeCom = 2;
|
//}
|
//else if (current.Y == neighbor.Y)
|
//{
|
// current.NodeCom = 3;
|
//}
|
|
//// 补充参数
|
//if (current.Equals(start))
|
//{
|
// current.NodeCom = moveType;
|
// current.IsSendPlc = true;
|
//}
|
//if (neighbor.Equals(end))
|
//{
|
// //neighbor.NodeCom = moveType != 0 ? 2:0 ;
|
// if (moveType == 1)
|
// {
|
// neighbor.NodeCom = 4;
|
// }
|
// else
|
// {
|
// neighbor.NodeCom = current.NodeCom;
|
// }
|
|
// neighbor.IsSendPlc = true;
|
//}
|
if (currentModel.Make != locationModel.Make)
|
{
|
neighbor.IsSendPlc = true;
|
}
|
|
|
// 更新实际距离与预估距离
|
cameFrom[neighbor] = current;
|
gScore[neighbor] = tentativeGScore;
|
fScore[neighbor] = tentativeGScore + Heuristic(neighbor, end);
|
openSet.Add((fScore[neighbor], neighbor));
|
isNextNode = true;
|
}
|
}
|
|
if (!isNextNode)
|
{
|
closeSet[current] = current;
|
}
|
}
|
#endregion
|
}
|
catch (Exception)
|
{
|
throw;
|
}
|
|
return null;
|
}
|
|
/// <summary>
|
/// 获取小车移动的目标位置
|
/// </summary>
|
/// <param name="startLocation">起始位置</param>
|
/// <param name="noPathStr">不能存放的储位字符串</param>
|
/// <returns></returns>
|
public static string GetCarEndLocation(string startLocation,string noPathStr)
|
{
|
if (string.IsNullOrEmpty(startLocation))
|
{
|
return null;
|
}
|
|
// 起始位置
|
CarModel start = new CarModel()
|
{
|
X = int.Parse(startLocation.Substring(0, 2)),
|
Y = int.Parse(startLocation.Substring(2, 2)),
|
Z = int.Parse(startLocation.Substring(4, 2)),
|
NodeCom = 0
|
};
|
|
// 获取储位表信息存储到集合里
|
var layer = int.Parse(startLocation.Substring(4, 2));
|
|
// 获取当前层储位信息
|
var locationModels = _db.Queryable<WcsStorageLocat>()
|
.Where(m => m.WareHouseNo == "W01" && m.Layer == layer && m.IsDelete == false)
|
.ToList();
|
|
#region MyRegion
|
|
var list = locationModels.Where(m => !noPathStr.Contains(m.LocatNo)).OrderBy(m=> Math.Abs(start.X - m.Row) + Math.Abs(start.Y - m.Column)).ToList();
|
var locateStr = "";
|
foreach (var item in list)
|
{
|
// 储位状态为损坏不可通行 储位状态为屏蔽为可通行不可存储托盘
|
if (item.Flag == "2")
|
{
|
continue;
|
}
|
locateStr = item.LocatNo;
|
break;
|
}
|
return locateStr;
|
#endregion
|
|
#region 使用算法计算小车可移动的目标路径
|
//try
|
//{
|
|
// // 定义关闭节点字典
|
// var closeSet = new Dictionary<CarModel, CarModel>();
|
|
// // 存储小车可运行的方向
|
// var validDirections = new List<CarModel>();
|
// var currentLocation = locationModels.FirstOrDefault(m => m.Row == start.X && m.Column == start.Y);
|
// if (currentLocation.Make == "0")
|
// {
|
// // 主通道
|
// validDirections.Add(new CarModel() { X = 1, Y = 0 }); // 右
|
// validDirections.Add(new CarModel() { X = -1, Y = 0 }); // 左
|
// validDirections.Add(new CarModel() { X = 0, Y = 1 }); // 下
|
// validDirections.Add(new CarModel() { X = 0, Y = -1 }); // 上
|
// }
|
|
// if (currentLocation.Make == "1")
|
// {
|
// // 子通道
|
// // 先拆分出口
|
// var outNode = currentLocation.AisleOne;
|
// if (string.IsNullOrEmpty(outNode))
|
// {
|
// throw new Exception("当前位置没有维护出口！");
|
// }
|
// int outX = int.Parse(outNode.Substring(0, 2));
|
// int outY = int.Parse(outNode.Substring(2, 2));
|
// if (start.X == outX)
|
// {
|
// validDirections.Add(new CarModel() { X = 0, Y = 1 }); // 下
|
// validDirections.Add(new CarModel() { X = 0, Y = -1 }); // 上
|
|
// }
|
// else
|
// {
|
// validDirections.Add(new CarModel() { X = 1, Y = 0 }); // 右
|
// validDirections.Add(new CarModel() { X = -1, Y = 0 }); // 左
|
// }
|
// }
|
|
// foreach (var dir in validDirections)
|
// {
|
// CarModel neighbor = new CarModel() { X = start.X + dir.X, Y = start.Y + dir.Y, Z = layer };
|
|
// // 验证下一节点位置是否可通行并且判断是否被其他小车占用
|
|
|
// // 判断下一节点是否关闭
|
// if (closeSet.ContainsKey(neighbor))
|
// {
|
// closeSet[neighbor] = neighbor;
|
// }
|
|
// // 当前节点
|
// var currentModel = locationModels.FirstOrDefault(it => it.Row == start.X && it.Column == start.Y);
|
// // 下一节点
|
// var locationModel = locationModels.FirstOrDefault(it => it.Row == neighbor.X && it.Column == neighbor.Y);
|
|
// // 不存在此位置信息
|
// if (locationModel == null)
|
// {
|
// closeSet[neighbor] = neighbor;
|
// continue;
|
// }
|
// // 储位状态为损坏不可通行 储位状态为屏蔽为可通行不可存储托盘
|
// if (locationModel.Flag == "2")
|
// {
|
// closeSet[neighbor] = neighbor;
|
// continue;
|
// }
|
|
// }
|
|
|
|
//}
|
//catch (Exception)
|
//{
|
// throw;
|
//}
|
|
//return null;
|
#endregion
|
}
|
|
public static int GetTaskNo()
|
{
|
var list = _db.Queryable<WcsCarTasks>().ToList();
|
var maxNo = list.Max(m => m.CarTaskNo);
|
|
if (maxNo!=null && maxNo > 0)
|
{
|
if (maxNo++ > 65535)
|
{
|
return 1;
|
}
|
|
return (int)maxNo++;
|
}
|
return 1;
|
|
}
|
|
/// <summary>
|
/// 计算曼哈顿距离
|
/// </summary>
|
/// <param name="start">起始位置</param>
|
/// <param name="end">目标位置</param>
|
/// <returns>位置距离</returns>
|
private static int Heuristic(CarModel start, CarModel end)
|
{
|
return Math.Abs(start.X - end.X) + Math.Abs(start.Y - end.Y);
|
}
|
|
/// <summary>
|
/// 重构完整路径
|
/// </summary>
|
/// <param name="cameFrom"></param>
|
/// <param name="current"></param>
|
/// <returns></returns>
|
private static List<CarModel> ReconstructPath(Dictionary<CarModel, CarModel> cameFrom, CarModel current)
|
{
|
var path = new List<CarModel> { current };
|
while (cameFrom.ContainsKey(current))
|
{
|
current = cameFrom[current];
|
path.Insert(0, current);
|
}
|
return path;
|
}
|
}
|
|
public class CarModel : IComparable<CarModel>
|
{
|
/// <summary>
|
/// 行=X
|
/// </summary>
|
public int X { get; set; }
|
|
/// <summary>
|
/// 列=Y
|
/// </summary>
|
public int Y { get; set; }
|
|
/// <summary>
|
/// 层=Z
|
/// </summary>
|
public int Z { get; set; }
|
|
/// <summary>
|
/// 节点命令 1:顶货 2:子通道运行 3:主通道运行 4:放货
|
/// </summary>
|
public int NodeCom { get; set; }
|
|
/// <summary>
|
/// 是否下发plc
|
/// </summary>
|
public bool IsSendPlc { get; set; }
|
|
public int CompareTo(CarModel other)
|
{
|
if (other == null)
|
return 1;
|
|
// 这里根据 X、Y、Z 坐标进行比较
|
int result = X.CompareTo(other.X);
|
if (result != 0)
|
return result;
|
|
result = Y.CompareTo(other.Y);
|
if (result != 0)
|
return result;
|
|
return Z.CompareTo(other.Z);
|
}
|
|
// 重写 Equals 方法
|
public override bool Equals(object obj)
|
{
|
if (obj == null || GetType() != obj.GetType())
|
return false;
|
|
CarModel other = (CarModel)obj;
|
return X == other.X && Y == other.Y && Z == other.Z;
|
}
|
|
// 重写 GetHashCode 方法
|
public override int GetHashCode()
|
{
|
return HashCode.Combine(X, Y, Z);
|
}
|
}
|
|
public class CarInfo
|
{
|
/// <summary>
|
/// 小车IP
|
/// </summary>
|
public string CarPlcIp { get; set; }
|
|
/// <summary>
|
/// 顺序等级
|
/// </summary>
|
public int Level { get; set; }
|
|
/// <summary>
|
/// 行=X
|
/// </summary>
|
public int X { get; set; }
|
|
/// <summary>
|
/// 列=Y
|
/// </summary>
|
public int Y { get; set; }
|
|
/// <summary>
|
/// 层=Z
|
/// </summary>
|
public int Z { get; set; }
|
}
|
