593 lines
24 KiB
C#
593 lines
24 KiB
C#
using AGVControl.Models;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Runtime.CompilerServices;
|
|
using System.Security.Cryptography.X509Certificates;
|
|
using System.Threading;
|
|
using static System.Windows.Forms.VisualStyles.VisualStyleElement.TextBox;
|
|
|
|
namespace AGVControl
|
|
{
|
|
public class MapControlManager
|
|
{
|
|
public AGVActionPrediction PredictResult = null;
|
|
public AGV agv = new AGV();
|
|
public List<RFIDPoint> RFIDPoints = new List<RFIDPoint>();
|
|
public HashSet<RFIDConnection> rfidConnections = new HashSet<RFIDConnection>();
|
|
|
|
private ManualResetEvent mrepredict = new ManualResetEvent(true);
|
|
|
|
public MapControlManager()
|
|
{
|
|
|
|
}
|
|
~MapControlManager()
|
|
{
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// 목표지정으로 모터방향이 이동하고 있는가?
|
|
/// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public bool? IsMotDirection_To_Target()
|
|
{
|
|
if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) return null;
|
|
if (agv.MainPath.Any() == false) return null;
|
|
|
|
|
|
var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
|
|
var lstpt = agv.MovementHistory.Last();
|
|
|
|
//현재 이후의 경로를 가져온다
|
|
var curidx = agv.MainPath.FindIndex(t => t.Value == lstpt.Value);
|
|
var preidx = agv.MainPath.FindIndex(t => t.Value == prept.Value);
|
|
if (curidx == -1 || preidx == -1) return null;
|
|
|
|
//지정된경로 반대방향으로 이동하고 있다
|
|
return preidx < curidx;
|
|
}
|
|
public float GetDistance(Point p1, Point p2)
|
|
{
|
|
float dx = p1.X - p2.X;
|
|
float dy = p1.Y - p2.Y;
|
|
return (float)Math.Sqrt(dx * dx + dy * dy); // double을 float로 명시적 캐스팅
|
|
}
|
|
|
|
public AGVActionPrediction PredictNextAction()
|
|
{
|
|
if (mrepredict.WaitOne(1) == false)
|
|
{
|
|
PredictResult = CreatePrediction("이전 작업이 완료되지 않았습니다",
|
|
AGVActionReasonCode.busy,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, false);
|
|
return PredictResult;
|
|
}
|
|
|
|
mrepredict.Reset();
|
|
try
|
|
{
|
|
// 0. 설정경로와 리프트 방향 체크 (경로설정이 없을때에는 직선이동경로내의 방향들과 체크한다)
|
|
agv.IsTargetDirectionMatch = IsLiftDirectionMatch() ?? false;
|
|
|
|
// 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정)
|
|
if (agv.CurrentRFID.Value == 0)
|
|
{
|
|
PredictResult = CreatePrediction("AGV 위치 미확정(처음 기동)",
|
|
AGVActionReasonCode.NoPosition,
|
|
AGVMoveState.Run,
|
|
Direction.Backward, true,
|
|
moveSpeed: AgvSpeed.Low,
|
|
moveDiv: AgvRunDirection.Straight);
|
|
return PredictResult;
|
|
}
|
|
|
|
//2. 이동방향을 모른다
|
|
if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2)
|
|
{
|
|
PredictResult = CreatePrediction("AGV이동방향 알수없음",
|
|
AGVActionReasonCode.NoDirection,
|
|
AGVMoveState.Run,
|
|
Direction.Backward, true,
|
|
moveSpeed: AgvSpeed.Low,
|
|
moveDiv: AgvRunDirection.Straight);
|
|
return PredictResult;
|
|
}
|
|
|
|
// 3. 경로가 없거나 현재 위치가 경로에 없음
|
|
if ((agv.MainPath?.Count ?? 0) < 2 || agv.MainPath.Last().Value != agv.TargetRFID.Value)
|
|
{
|
|
//목적지가 없다면 진행할 수 없다
|
|
if (agv.TargetRFID == null)
|
|
{
|
|
PredictResult = CreatePrediction("경로 없음 또는 현재 위치 미확정",
|
|
AGVActionReasonCode.NoPath,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true);
|
|
return PredictResult;
|
|
}
|
|
else
|
|
{
|
|
//목적지가 있는데 경로가 없다면 경로 예측을 진행해야한다.
|
|
var CurPt = agv.MovementHistory.Last();
|
|
var prlt = CalculatePath(CurPt, agv.TargetRFID);
|
|
if (prlt.Success == false)
|
|
{
|
|
PredictResult = CreatePrediction("목적지 경로 예측 실패",
|
|
AGVActionReasonCode.Unknown,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true,
|
|
nextRFID: agv.TargetRFID);
|
|
return PredictResult;
|
|
}
|
|
else
|
|
{
|
|
//신규목적지에 대한 경로예측이 성공했다
|
|
agv.SubPath.Clear();
|
|
agv.MainPath = prlt.Path;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 4. 경로상에서 다음 RFID 예측
|
|
int idx = agv.MainPath.FindIndex(p => p.Value == agv.CurrentRFID.Value);
|
|
if (idx < 0)
|
|
{
|
|
PredictResult = CreatePrediction("현재 위치가 경로에 없음",
|
|
AGVActionReasonCode.NotOnPath,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true);
|
|
return PredictResult;
|
|
}
|
|
|
|
|
|
|
|
// 4. 목적지 도달 전, 회전이 필요한경우인가?
|
|
// 목적지 RFID 정보
|
|
var destRFID = agv.MainPath.Last();
|
|
|
|
//리프트 방향이 맞는가?
|
|
var IsLiftDir = IsLiftDirectionMatch() ?? false;
|
|
|
|
//모션이동방향이 맞는가?
|
|
var IsMotDir = IsMotDirection_To_Target() ?? false;
|
|
|
|
var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
|
|
var curPT = agv.MovementHistory.Last();
|
|
|
|
//리프트방향이 맞지 않다면 회전가능한 위치로 이동을 해야한다
|
|
if (IsLiftDir == false)
|
|
{
|
|
AgvSpeed? agv_spd = null;
|
|
AgvRunDirection? agv_dir = null;
|
|
|
|
//회전가능한 위치로 이동을 해야한다
|
|
|
|
//1. 가까운 회전위치를 찾는다
|
|
var nearTurnPoint = RFIDPoints.Where(t => t.IsRotatable)?.OrderBy(t => GetDistance(t.Location, agv.CurrentRFID.Location)).FirstOrDefault() ?? null;
|
|
if (nearTurnPoint == null)
|
|
{
|
|
PredictResult = CreatePrediction("회전 가능한 위치가 없습니다",
|
|
AGVActionReasonCode.NoTurnPoint,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true);
|
|
return PredictResult;
|
|
}
|
|
|
|
//2. 이동하기위한 경로계산 및 이동을 한다 (생성조건)
|
|
//2-1. 서브경로가없는경우
|
|
//2-2. 시작과 종료번호가 다른 경우(경로가 변경이 되는 조건이다)
|
|
if (agv.CurrentRFID.Value != nearTurnPoint.Value)
|
|
{
|
|
if (agv.SubPath.Any() == false || agv.SubPath.Count < 2 ||
|
|
agv.SubPath.First().Value != agv.CurrentRFID.Value ||
|
|
agv.SubPath.Last().Value != nearTurnPoint.Value)
|
|
{
|
|
var rlt = CalculatePath(agv.CurrentRFID, nearTurnPoint); //이전포인트도 추가를 해준다
|
|
if (rlt.Success) agv.SubPath = rlt.Path;
|
|
else
|
|
{
|
|
agv.SubPath.Clear();
|
|
PredictResult = CreatePrediction("회전 위치까지의 경로를 계산할 수 없습니다",
|
|
AGVActionReasonCode.PathCalcError,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true,
|
|
nextRFID: nearTurnPoint);
|
|
return PredictResult;
|
|
}
|
|
}
|
|
|
|
|
|
//현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
|
|
var curidx = agv.SubPath.FindIndex(t => t.Value == curPT.Value);
|
|
var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value);
|
|
Direction newdirection = agv.CurrentMOTDirection;
|
|
string message = "턴위치로 이동중";
|
|
|
|
|
|
|
|
if (preidx > curidx)
|
|
{
|
|
//지정경로를 거꾸로 이동하고 있다
|
|
if (agv.CurrentMOTDirection == Direction.Forward)
|
|
newdirection = Direction.Backward;
|
|
else
|
|
newdirection = Direction.Forward;
|
|
message += "(방향전환)";
|
|
|
|
|
|
}
|
|
|
|
//도로정보를 확인하여 속도와 분기명령을 실행한다
|
|
var roadinfo = GetRoadInfo(agv.SubPath, curPT);
|
|
agv_spd = roadinfo.spd;
|
|
agv_dir = roadinfo.dir;
|
|
|
|
PredictResult = CreatePrediction(message,
|
|
AGVActionReasonCode.MoveForTurn,
|
|
AGVMoveState.Run,
|
|
newdirection, true,
|
|
moveSpeed: agv_spd,
|
|
moveDiv: agv_dir,
|
|
nextRFID: nearTurnPoint);
|
|
return PredictResult;
|
|
}
|
|
|
|
var roadinfo2 = GetRoadInfo(agv.SubPath.Any() ? agv.SubPath : agv.MainPath, curPT);
|
|
agv_spd = roadinfo2.spd;
|
|
agv_dir = roadinfo2.dir;
|
|
|
|
|
|
PredictResult = CreatePrediction("턴 완료 대기",
|
|
AGVActionReasonCode.NeedTurn,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true,
|
|
moveSpeed: agv_spd,
|
|
moveDiv: agv_dir,
|
|
nextRFID: nearTurnPoint);
|
|
return PredictResult;
|
|
}
|
|
|
|
//보조이동선제거
|
|
if (agv.SubPath != null && agv.SubPath.Any())
|
|
agv.SubPath.Clear();
|
|
|
|
//3. 목적지위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다
|
|
if (agv.CurrentRFID.Value != destRFID.Value)
|
|
{
|
|
//현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
|
|
var curidx = agv.MainPath.FindIndex(t => t.Value == curPT.Value);
|
|
var preidx = agv.MainPath.FindIndex(t => t.Value == PrePT.Value);
|
|
Direction newdirection = agv.CurrentMOTDirection;
|
|
string message = "목적지 이동중";
|
|
if (preidx > curidx)
|
|
{
|
|
//지정경로를 거꾸로 이동하고 있다
|
|
if (agv.CurrentMOTDirection == Direction.Forward)
|
|
newdirection = Direction.Backward;
|
|
else
|
|
newdirection = Direction.Forward;
|
|
message += "(방향전환)";
|
|
}
|
|
|
|
//경로상 바로 다음 위치를 확인한다
|
|
var nexstRFID = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == curPT.Value) + 1).First();
|
|
var roadinfo = GetRoadInfo(agv.MainPath, curPT);
|
|
|
|
|
|
PredictResult = CreatePrediction(message,
|
|
AGVActionReasonCode.Normal,
|
|
AGVMoveState.Run,
|
|
newdirection, true,
|
|
moveSpeed: roadinfo.spd,
|
|
moveDiv: roadinfo.dir,
|
|
nextRFID: nexstRFID);
|
|
return PredictResult;
|
|
}
|
|
|
|
|
|
// 5. 목적지 도달 시
|
|
PredictResult = CreatePrediction("경로의 마지막 지점(목적지 도달)",
|
|
AGVActionReasonCode.Arrived,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true,
|
|
nextRFID: destRFID);
|
|
return PredictResult;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
|
|
PredictResult = CreatePrediction($"ERR:{ex.Message}",
|
|
AGVActionReasonCode.Unknown,
|
|
AGVMoveState.Stop,
|
|
agv.CurrentMOTDirection, true);
|
|
return PredictResult;
|
|
}
|
|
finally
|
|
{
|
|
mrepredict.Set();
|
|
}
|
|
|
|
}
|
|
|
|
(AgvSpeed? spd, AgvRunDirection? dir, RFIDConnection info) GetRoadInfo(List<RFIDPoint> paths, RFIDPoint curPT)
|
|
{
|
|
//도로정보를 확인하여 속도와 분기명령을 실행한다
|
|
AgvSpeed? agv_spd = null;
|
|
AgvRunDirection? agv_div = null;
|
|
RFIDConnection info = null;
|
|
|
|
var nextpt = paths.Skip(paths.FindIndex(t => t.Value == curPT.Value) + 1).FirstOrDefault();
|
|
if (nextpt != null)
|
|
{
|
|
var p1 = rfidConnections.Where(t => t.P1.Value == curPT.Value && t.P2.Value == nextpt.Value).FirstOrDefault();
|
|
if (p1 != null)
|
|
{
|
|
//positive
|
|
agv_spd = p1.MoveSpeedP;
|
|
agv_div = p1.MoveDirectionP;
|
|
info = p1;
|
|
}
|
|
|
|
var p2 = rfidConnections.Where(t => t.P2.Value == curPT.Value && t.P1.Value == nextpt.Value).FirstOrDefault();
|
|
if (p2 != null)
|
|
{
|
|
//negative
|
|
agv_spd = p2.MoveSpeedN;
|
|
agv_div = p2.MoveDirectionN;
|
|
info = p2;
|
|
}
|
|
}
|
|
return (agv_spd, agv_div, info);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이웃포인터를 반환합니다
|
|
/// </summary>
|
|
/// <param name="point"></param>
|
|
/// <returns></returns>
|
|
public List<RFIDPoint> GetNeighbors(RFIDPoint pt)
|
|
{
|
|
var neighbors = new List<RFIDPoint>();
|
|
|
|
//값이 없는 경우 오류 반환
|
|
if (pt == null) return neighbors;
|
|
|
|
//연결정보에서 데이터를 찾은 후 반환한다
|
|
foreach (var connection in rfidConnections)
|
|
{
|
|
RFIDPoint nPT = null;
|
|
if (connection.P1.Value == pt.Value)
|
|
{
|
|
nPT = connection.P2;
|
|
}
|
|
else if (connection.P2.Value == pt.Value)
|
|
{
|
|
nPT = connection.P1;
|
|
}
|
|
|
|
if (nPT != null) neighbors.Add(nPT);
|
|
}
|
|
|
|
//중복제거후 반한
|
|
return neighbors.Distinct().ToList();
|
|
}
|
|
|
|
public RFIDPoint FindRFIDPoint(uint rfidValue)
|
|
{
|
|
if (RFIDPoints == null || RFIDPoints.Any() == false) return null;
|
|
return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue);
|
|
}
|
|
private float Heuristic(Point a, Point b)
|
|
{
|
|
return (float)Math.Sqrt(Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2));
|
|
}
|
|
|
|
private PathResult ReconstructPath(Dictionary<uint, RFIDPoint> cameFrom, RFIDPoint current)
|
|
{
|
|
var path = new List<RFIDPoint> { current };
|
|
while (cameFrom.ContainsKey(current.Value))
|
|
{
|
|
current = cameFrom[current.Value];
|
|
if (path.Contains(current) == false)
|
|
path.Insert(0, current);
|
|
else
|
|
break; //왜인지 반복되는경우가있어 종료한다
|
|
}
|
|
return new PathResult
|
|
{
|
|
Path = path,
|
|
};
|
|
}
|
|
public PathResult CalculatePath(RFIDPoint start, RFIDPoint end)
|
|
{
|
|
var openList = new List<RFIDPoint> { start };
|
|
var closedList = new List<RFIDPoint>();
|
|
var cameFrom = new Dictionary<uint, RFIDPoint>();
|
|
var gScore = new Dictionary<RFIDPoint, float> { { start, 0 } };
|
|
var fScore = new Dictionary<RFIDPoint, float> { { start, Heuristic(start.Location, end.Location) } };
|
|
|
|
|
|
|
|
if (start.Value == end.Value)
|
|
{
|
|
return new PathResult
|
|
{
|
|
Message = "시작위치와 대상위치가 동일 합니다",
|
|
Path = null,
|
|
};
|
|
}
|
|
|
|
//if (autorun) openList.Clear();
|
|
|
|
|
|
while (openList.Count > 0)
|
|
{
|
|
var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First();
|
|
|
|
if (current.Value == end.Value)
|
|
{
|
|
return ReconstructPath(cameFrom, current);
|
|
}
|
|
|
|
openList.Remove(current);
|
|
closedList.Add(current);
|
|
|
|
var nb = GetNeighbors(current);
|
|
foreach (var neighbor in nb)
|
|
{
|
|
if (closedList.Contains(neighbor))
|
|
continue;
|
|
|
|
float tentativeGScore = gScore[current] + GetDistance(current.Location, neighbor.Location);
|
|
|
|
if (!openList.Contains(neighbor))
|
|
openList.Add(neighbor);
|
|
else if (tentativeGScore >= gScore[neighbor])
|
|
continue;
|
|
|
|
cameFrom[neighbor.Value] = (RFIDPoint)current;
|
|
gScore[neighbor] = tentativeGScore;
|
|
fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor.Location, end.Location);
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return new PathResult
|
|
{
|
|
Path = openList,
|
|
};
|
|
|
|
}
|
|
public PathResult CalculatePath(uint tagStrt, uint tagEnd)
|
|
{
|
|
var retval = new PathResult
|
|
{
|
|
Message = string.Empty,
|
|
Path = new List<RFIDPoint>(),
|
|
};
|
|
|
|
var startPoint = FindRFIDPoint(tagStrt);
|
|
var endPoint = FindRFIDPoint(tagEnd);
|
|
if (startPoint == null || endPoint == null)
|
|
{
|
|
retval.Message = "유효한 RFID 값을 입력해주세요.";
|
|
return retval;
|
|
}
|
|
|
|
retval = CalculatePath(startPoint, endPoint);
|
|
if (retval.Success == false)
|
|
retval.Message = "경로를 찾을 수 없습니다";
|
|
|
|
return retval;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 리프트방향과 대상위치와의 방향이 일치하는가?
|
|
/// 목적지경로가 셋팅된 경우 현재 이동방향이 목적지방향과 일치하는가?
|
|
/// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public bool? IsLiftDirectionMatch()
|
|
{
|
|
|
|
if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1)
|
|
{
|
|
RFIDPoint TargetPT = null;
|
|
var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
|
|
var lstpt = agv.MovementHistory.Last();
|
|
|
|
//뒤로이동하는경우라면 이전위치에 리프트가 있다.
|
|
if (lstpt.Direction == Direction.Backward)
|
|
{
|
|
TargetPT = prept;
|
|
}
|
|
else //앞으로 이동한다면 이동방향과 동일하다
|
|
{
|
|
//이전위치는 제거 하고 처음발견된 것을 대상으로 한다
|
|
TargetPT = GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault();
|
|
}
|
|
|
|
//목적지가 있다면 목적지의 방향과 일치하는지 확인해야한다
|
|
//남은경로중에 방향이 고정된 핀이 있다면 그것과 일치하는지 확인해야 한다
|
|
if (agv.MainPath.Any())
|
|
{
|
|
//지정된경로 반대방향으로 이동하고 있다
|
|
if ((IsMotDirection_To_Target() ?? false) == false)
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
var nextRoutes = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == lstpt.Value) + 1).ToList();
|
|
var DirectionMatch = true;
|
|
foreach (var item in nextRoutes)
|
|
{
|
|
if (item.FixedDirection != null && item.FixedDirection != lstpt.Direction)
|
|
{
|
|
DirectionMatch = false;
|
|
break;
|
|
}
|
|
}
|
|
return DirectionMatch;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//대상포인트와의 방향만 체크한다.
|
|
//고정대상이없다면 방향이 맞는것으로 한다
|
|
return (TargetPT.FixedDirection ?? lstpt.Direction) == lstpt.Direction;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//이동된경로정보가 없다면 리프트 방향을 체크할 수 없으므로 대상과 위치가 맞지 않는걸로 기본값을 설정한다
|
|
//이렇게 설정하면 대상으로 이동불가하고 뒤로 가도록 유도된다
|
|
return null;
|
|
}
|
|
}
|
|
|
|
// Changed 속성 설정을 위한 헬퍼 메서드
|
|
private bool IsPredictionChanged(AGVActionPrediction newPrediction)
|
|
{
|
|
if (PredictResult == null) return true; // 이전 예측이 없으면 변경됨
|
|
|
|
// Idx와 Changed를 제외하고 비교
|
|
return !PredictResult.Equals(newPrediction);
|
|
}
|
|
|
|
private AGVActionPrediction CreatePrediction(
|
|
string reason,
|
|
AGVActionReasonCode reasonCode,
|
|
AGVMoveState moveState,
|
|
Direction direction, bool IDXUpdate = true,
|
|
AgvSpeed? moveSpeed = null,
|
|
AgvRunDirection? moveDiv = null,
|
|
RFIDPoint nextRFID = null
|
|
)
|
|
{
|
|
var newPrediction = new AGVActionPrediction
|
|
{
|
|
NextRFID = nextRFID,
|
|
Reason = reason,
|
|
ReasonCode = reasonCode,
|
|
MoveState = moveState,
|
|
Direction = direction,
|
|
MoveSpeed = moveSpeed,
|
|
MoveDiv = moveDiv,
|
|
Idx = IDXUpdate ? (PredictResult?.Idx + 1 ?? 1) : (PredictResult?.Idx ?? 0)
|
|
};
|
|
|
|
newPrediction.Changed = IsPredictionChanged(newPrediction);
|
|
return newPrediction;
|
|
}
|
|
|
|
}
|
|
}
|