diff --git a/Cs_HMI/SubProject/AGVControl/MapControl.cs b/Cs_HMI/SubProject/AGVControl/MapControl.cs index 2107526..03db220 100644 --- a/Cs_HMI/SubProject/AGVControl/MapControl.cs +++ b/Cs_HMI/SubProject/AGVControl/MapControl.cs @@ -24,7 +24,6 @@ namespace AGVControl public bool IsBidirectional { get; set; } public float Distance { get; set; } public List IntermediateRFIDs { get; set; } = new List(); - public override bool Equals(object obj) { @@ -42,63 +41,99 @@ namespace AGVControl return StartRFID.GetHashCode() ^ EndRFID.GetHashCode(); } } + public struct PathResult + + { + public bool Success { get; set; } + public string Message { get; set; } + public List Path { get; set; } + } + + public enum AGVActionReasonCode + { + Unknown = 0, + NoPosition, // 위치 미확정(처음 기동) + NoPath, // 경로 없음 또는 현재 위치 미확정 + NotOnPath, // 현재 위치가 경로에 없음 + Arrived, // 경로의 마지막 지점(목적지 도달) + Normal, // 정상(다음 RFID 있음) + NeedTurn + } + + public class AGVActionPrediction + { + public Direction Direction { get; set; } + public uint? NextRFID { get; set; } + public string Reason { get; set; } + public AGVActionReasonCode ReasonCode { get; set; } + } public partial class MapControl : Control { + #region 상수 정의 + private const int SNAP_DISTANCE = 10; // 점 근접 거리 + private const int LINE_WIDTH = 20; // 선 굵기 + private const int TOOLBAR_WIDTH = 58; // 툴바 너비 + private const int TOOLBAR_WIDTHR = 78; // 우측 툴바 너비 + private const int TOOLBAR_BUTTON_HEIGHT = 40; // 툴바 버튼 높이 + private const int TOOLBAR_MARGIN = 5; // 툴바 마진 + private const int SELECTION_DISTANCE = 15; // 선택 가능 거리 + #endregion + + #region 멤버 변수 + // 맵 데이터 private List rfidPoints; private List mapTexts; private List customLines; private List rfidLines; private HashSet rfidConnections; public AGV agv; + + // 화면 조작 관련 private float zoom = 1.0f; private PointF offset = PointF.Empty; private Point lastMousePosition; + private Point currentMousePosition; private bool isDragging = false; private Point? previewStartPoint = null; - private Point currentMousePosition; - private const int SNAP_DISTANCE = 10; // 점 근접 거리 - private const int LINE_WIDTH = 20; // 선 굵기 - private const int TOOLBAR_WIDTH = 58; - private const int TOOLBAR_WIDTHR = 78; - private const int TOOLBAR_BUTTON_HEIGHT = 40; - private const int TOOLBAR_MARGIN = 5; - - private List toolbarRects; - + private Point? lineStartPoint = null; + private Point? branchPoint = null; + // 모드 관련 private bool isAddingText = false; private bool isAddingPoint = false; private bool isDrawingCustomLine = false; private bool isDrawingRFIDLine = false; private bool isDeletingRFIDLine = false; private bool isDrawingLine = false; + private bool isDraggingPoint = false; + private bool isDraggingText = false; + private Point dragOffset; + + // 선택된 객체 private MapText selectedText = null; private CustomLine selectedLine = null; private RFIDPoint selectedRFID = null; private RFIDLine selectedRFIDLine = null; private Point? draggingPoint = null; - private bool isDraggingPoint = false; - private const int SELECTION_DISTANCE = 15; // 선택 가능 거리를 늘림 - private bool isDraggingText = false; - private Point dragOffset; - // private List currentPath; + // 툴바 관련 + private List toolbarRects; + // RFID 관련 + public string RFIDStartNo { get; set; } = string.Empty; + public int RFIDLastNumber = 0; + private string filename = string.Empty; + #endregion + + #region 속성 public MapText SelectedText => selectedText; public CustomLine SelectedLine => selectedLine; public RFIDPoint SelectedRFID => selectedRFID; public RFIDLine SelectedRFIDLine => selectedRFIDLine; + #endregion - - - 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로 명시적 캐스팅 - } - + #region 생성자 및 초기화 public MapControl() { this.DoubleBuffered = true; @@ -109,446 +144,15 @@ namespace AGVControl rfidConnections = new HashSet(); agv = new AGV(); - // 툴바 버튼 영역 초기화 UpdateToolbarRects(); - - } - - protected override void OnResize(EventArgs e) - { - base.OnResize(e); - UpdateToolbarRects(); - this.Invalidate(); - } - - private void UpdateToolbarRects() - { - int x, y, c, row; - var idx = 0; - - if (this.toolbarRects == null) this.toolbarRects = new List(); - else this.toolbarRects.Clear(); - - //left toolbar - x = TOOLBAR_MARGIN; - y = 10; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "+", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "-", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "1:1", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Cut", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - - //right toolbar - y = 10; - row = 0; - x = DisplayRectangle.Right - TOOLBAR_WIDTHR - TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Text", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Line", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Point", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Magnet", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Load", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Save", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Pos", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; - toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Path", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); - - } - - public void SetPreviewStartPoint(Point? point) - { - previewStartPoint = point; - this.Invalidate(); - } - - public void UpdatePreviewLine(Point currentPosition) - { - currentMousePosition = currentPosition; - this.Invalidate(); - } - - private Point SnapToPoint(Point point) - { - // RFID 포인트와 근접한지 확인 - foreach (var rfid in rfidPoints) - { - if (GetDistance(point, rfid.Location) <= SNAP_DISTANCE) - { - return rfid.Location; - } - } - - return point; - } - - // 화면 좌표를 실제 맵 좌표로 변환 - public Point ScreenToMap(Point screenPoint) - { - int adjustedX = screenPoint.X; - return new Point( - (int)((adjustedX - offset.X) / zoom), - (int)((screenPoint.Y - offset.Y) / zoom) - ); - } - - public RFIDPoint FindRFIDPoint(uint rfidValue) - { - if (rfidPoints == null || rfidPoints.Any() == false) return null; - return rfidPoints.FirstOrDefault(r => r.RFIDValue == rfidValue); - } - - public bool SetCurrentPosition(uint rfidValue) - { - var rfidPoint = FindRFIDPoint(rfidValue); - if (rfidPoint != null) - { - agv.CurrentPosition = rfidPoint.Location; - agv.CurrentRFID = rfidValue; - - // 목적지가 설정되어 있고 경로가 있는 경우 검증 - if (agv.TargetPosition != Point.Empty && agv.CurrentPath.Count > 0) - { - // 현재 위치가 경로에 있는지 확인 - bool isOnPath = agv.CurrentPath.Contains(agv.CurrentPosition); - - if (!isOnPath) - { - // 경로를 벗어났으므로 새로운 경로 계산 - var pathResult = CalculatePath(agv.CurrentRFID, agv.TargetRFID); - if (pathResult.Success) - { - SetCurrentPath(pathResult.Path); - } - } - - // AGV의 방향 결정 - if (agv.CurrentPath.Count > 0) - { - // 다음 목적지 찾기 - var nextPoint = agv.CurrentPath[0]; - - // 회전 가능 여부를 고려하여 방향 결정 - agv.TargetDirection = DetermineDirection(agv.CurrentPosition, nextPoint, agv.TargetPosition); - } - } - - // 목적지 RFID에 도착했고, 해당 RFID에 고정방향이 있으면 TargetDirection을 강제 설정 - if (agv.TargetRFID == rfidValue) - { - var destRFID = FindRFIDPoint(rfidValue); - if (destRFID != null && destRFID.FixedDirection.HasValue) - { - agv.TargetDirection = destRFID.FixedDirection.Value; - } - } - - this.Invalidate(); - return true; - } - return false; - } - - public bool SetTargetPosition(uint rfidValue) - { - var rfidPoint = FindRFIDPoint(rfidValue); - if (rfidPoint != null) - { - agv.TargetPosition = rfidPoint.Location; - agv.TargetRFID = rfidValue; - this.Invalidate(); - return true; - } - return false; - } - - protected override void OnMouseWheel(MouseEventArgs e) - { - base.OnMouseWheel(e); - if (e.Delta > 0) - { - zoom *= 1.1f; - } - else - { - zoom /= 1.1f; - } - - zoom = Math.Max(0.1f, Math.Min(10.0f, zoom)); - this.Invalidate(); - } - - - protected override void OnMouseDown(MouseEventArgs e) - { - base.OnMouseDown(e); - lastMousePosition = e.Location; - var mapPoint = ScreenToMap(e.Location); - - if (e.Button == MouseButtons.Middle) - { - isDragging = true; - this.Cursor = Cursors.SizeAll; - } - else if (e.Button == MouseButtons.Left && !isAddingText && !isDrawingCustomLine && !isDrawingRFIDLine) - { - isDragging = true; - - // 텍스트 선택 및 드래그 시작 - foreach (var text in mapTexts) - { - var textSize = CreateGraphics().MeasureString(text.Text, text.Font); - var rect = new RectangleF( - text.Location.X - SELECTION_DISTANCE, - text.Location.Y - SELECTION_DISTANCE, - textSize.Width + SELECTION_DISTANCE * 2, - textSize.Height + SELECTION_DISTANCE * 2 - ); - - if (rect.Contains(mapPoint)) - { - selectedText = text; - isDraggingText = true; - // 드래그 시작점과 텍스트 위치의 차이를 저장 - dragOffset = new Point( - text.Location.X - mapPoint.X, - text.Location.Y - mapPoint.Y - ); - return; - } - } - - // 커스텀 라인 선택 및 드래그 포인트 설정 - foreach (var line in customLines) - { - float startDistance = GetDistance(mapPoint, line.StartPoint); - float endDistance = GetDistance(mapPoint, line.EndPoint); - - if (startDistance < SELECTION_DISTANCE) - { - selectedLine = line; - draggingPoint = line.StartPoint; - isDraggingPoint = true; - return; - } - else if (endDistance < SELECTION_DISTANCE) - { - selectedLine = line; - draggingPoint = line.EndPoint; - isDraggingPoint = true; - return; - } - } - - - // RFID 포인트 선택 - var clickedRFID = rfidPoints.FirstOrDefault(r => - GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); - if (clickedRFID != null) - { - selectedRFID = clickedRFID; - draggingPoint = clickedRFID.Location; - isDraggingPoint = true; - return; - } - - - } - } - - Point? lineStartPoint = null; - Point? branchPoint = null; - - - - protected override void OnMouseMove(MouseEventArgs e) - { - base.OnMouseMove(e); - currentMousePosition = e.Location; - var mapPoint = ScreenToMap(e.Location); - - if ((mousemode == eMouseMode.addcustomline || mousemode == eMouseMode.addrfidline) && branchPoint.HasValue) - { - UpdatePreviewLine(e.Location); - } - - // 툴바 버튼 호버 상태 업데이트 - var oldHovering = toolbarRects.OrderBy(t => t.Idx).Select(t => t.isHovering).ToArray(); - - //toolbar check - toolbarRects.ForEach(t => t.isHovering = t.Bounds.Contains(e.Location)); - - //hovering check - if (toolbarRects.Where(t => t.isHovering).Any()) - { - this.Cursor = Cursors.Hand; - } - else if (isDeletingRFIDLine) - { - this.Cursor = GetScissorsCursor(); - } - else - { - this.Cursor = Cursors.Default; - } - - //hovering display update - if (toolbarRects.Where(t => t.Dirty).Any()) - { - this.Invalidate(); - } - - if (isDragging) - { - if (e.Button == MouseButtons.Middle) - { - offset = new PointF( - offset.X + (e.Location.X - lastMousePosition.X), - offset.Y + (e.Location.Y - lastMousePosition.Y) - ); - } - else if (isDraggingText && selectedText != null) - { - // 텍스트 이동 - 드래그 오프셋 적용 - selectedText.Location = new Point( - mapPoint.X + dragOffset.X, - mapPoint.Y + dragOffset.Y - ); - } - else if (isDraggingPoint && draggingPoint.HasValue) - { - // 포인트 이동 - var delta = new Point( - mapPoint.X - ScreenToMap(lastMousePosition).X, - mapPoint.Y - ScreenToMap(lastMousePosition).Y - ); - - if (selectedLine != null) - { - // 커스텀 라인 포인트 이동 - if (draggingPoint.Value == selectedLine.StartPoint) - { - selectedLine.StartPoint = new Point( - selectedLine.StartPoint.X + delta.X, - selectedLine.StartPoint.Y + delta.Y - ); - } - else if (draggingPoint.Value == selectedLine.EndPoint) - { - selectedLine.EndPoint = new Point( - selectedLine.EndPoint.X + delta.X, - selectedLine.EndPoint.Y + delta.Y - ); - } - } - else if (selectedRFID != null) // RFID 포인트 이동 - { - // RFID 포인트 위치 업데이트 - selectedRFID.Location = new Point( - selectedRFID.Location.X + delta.X, - selectedRFID.Location.Y + delta.Y - ); - - // 연결된 RFID 라인 업데이트 - foreach (var line in rfidLines) - { - if (line.StartPoint == draggingPoint.Value) - { - line.StartPoint = selectedRFID.Location; - } - if (line.EndPoint == draggingPoint.Value) - { - line.EndPoint = selectedRFID.Location; - } - } - } - } - - lastMousePosition = e.Location; - this.Invalidate(); - } - - // 미리보기 라인 업데이트를 위한 마우스 위치 저장 - if (isDrawingRFIDLine || isDrawingCustomLine || previewStartPoint.HasValue) - { - currentMousePosition = mapPoint; - } - - this.Invalidate(); - } - private List CalculatePath(Point start, Point end) - { - var openList = new List { start }; - var closedList = new List(); - var cameFrom = new Dictionary(); - var gScore = new Dictionary { { start, 0 } }; - var fScore = new Dictionary { { start, Heuristic(start, end) } }; - - while (openList.Count > 0) - { - var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First(); - - if (current == end) - { - return ReconstructPath(cameFrom, current); - } - - openList.Remove(current); - closedList.Add(current); - - foreach (var neighbor in GetNeighbors(current)) - { - if (closedList.Contains(neighbor)) - continue; - - float tentativeGScore = gScore[current] + Distance(current, neighbor); - - if (!openList.Contains(neighbor)) - openList.Add(neighbor); - else if (tentativeGScore >= gScore[neighbor]) - continue; - - cameFrom[neighbor] = current; - gScore[neighbor] = tentativeGScore; - fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor, end); - } - } - - return null; - } - private List ReconstructPath(Dictionary cameFrom, Point current) - { - var path = new List { current }; - while (cameFrom.ContainsKey(current)) - { - current = cameFrom[current]; - path.Insert(0, current); - } - return path; - } - 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)); } + #endregion + #region 이벤트 핸들러 + public event EventHandler OnRightClick; + #endregion + #region OVERRIDE protected override void OnMouseUp(MouseEventArgs e) { base.OnMouseUp(e); @@ -564,125 +168,6 @@ namespace AGVControl isDraggingText = false; } } - private float Distance(Point a, Point b) - { - // RFID 라인을 통한 연결 확인 - var rfidLines = GetRFIDLines(); - var directConnection = rfidLines.FirstOrDefault(line => - (line.StartPoint == a && line.EndPoint == b) || - (line.StartPoint == b && line.EndPoint == a)); - - if (directConnection != null) - { - return directConnection.Distance; - } - - // 직접 연결되지 않은 경우 매우 큰 값 반환 - return float.MaxValue; - } - - private List GetNeighbors(Point point) - { - var neighbors = new List(); - var rfidLines = GetRFIDLines(); - - // RFID 라인에서 이웃 노드 찾기 - foreach (var line in rfidLines) - { - if (line.StartPoint == point) - { - neighbors.Add(line.EndPoint); - } - else if (line.EndPoint == point) - { - // 양방향 연결인 경우에만 시작점을 이웃으로 추가 - neighbors.Add(line.StartPoint); - } - } - - return neighbors.Distinct().ToList(); - } - public void SetPath(List rfids) - { - if (rfids == null || rfids.Count == 0) - return; - - var path = new List(); - foreach (var rfid in rfids) - { - var point = GetRFIDPoints().FirstOrDefault(p => p.RFIDValue == rfid); - if (point != null) - { - path.Add(point.Location); - } - } - - if (path.Count > 0) - { - SetCurrentPath(path); - } - } - - - public string RFIDStartNo { get; set; } = string.Empty; - public int RFIDLastNumber = 0; - string filename = string.Empty; - - - - - public struct PathResult - { - public bool Success { get; set; } - public string Message { get; set; } - public List Path { get; set; } - } - public PathResult CalculatePath(uint tagStrt, uint tagEnd) - { - var retval = new PathResult - { - Message = string.Empty, - Success = false, - Path = new List(), - }; - - var sp = tagStrt; //만약시작위치가 없다면 항상 충전기 기준으로 한다 - var ep = tagEnd; - - var startPoint = FindRFIDPoint(sp); - var endPoint = FindRFIDPoint(ep); - - if (startPoint == null || endPoint == null) - { - retval.Message = "유효한 RFID 값을 입력해주세요."; - return retval; - } - - retval.Path = CalculatePath(startPoint.Location, endPoint.Location); - if (retval.Path != null && retval.Path.Any()) - { - //SetCurrentPath(retval.Path); - - //// 경로 상의 모든 RFID 값을 가져옴 - //var rfidPath = new List(); - //foreach (var point in path) - //{ - // var rfid = GetRFIDPoints() - // .FirstOrDefault(r => r.Location == point); - // if (rfid != null) - // { - // rfidPath.Add(rfid.RFIDValue); - // } - //} - retval.Success = true; - } - else - { - retval.Message = "경로를 찾을 수 없습니다"; - } - return retval; - } - protected override void OnMouseClick(MouseEventArgs e) { base.OnMouseClick(e); @@ -891,6 +376,9 @@ namespace AGVControl }; rfidLines.Add(line); selectedRFIDLine = line; + + // RFID 연결 정보 처리 + ProcessRFIDConnections(); } // 다음 라인을 위해 현재 클릭한 RFID를 시작점으로 설정 previewStartPoint = clickedRFID.Location; @@ -901,6 +389,581 @@ namespace AGVControl } } } + protected override void OnMouseDoubleClick(MouseEventArgs e) + { + base.OnMouseDoubleClick(e); + + if (e.Button == MouseButtons.Left) + { + var mapPoint = ScreenToMap(e.Location); + + //RFID 포인트 찾기 + var selected_rfid = rfidPoints.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); + if (selected_rfid != null) + { + var oldtagno = selected_rfid.RFIDValue; + AR.UTIL.ShowProperty(selected_rfid); + if (selected_rfid.RFIDValue != oldtagno) + { + var items1 = this.rfidConnections.Where(t=>t.StartRFID == oldtagno); + foreach (var item in items1) + item.StartRFID = selected_rfid.RFIDValue; + + var items2 = this.rfidConnections.Where(t => t.EndRFID == oldtagno); + foreach (var item in items2) + item.EndRFID = selected_rfid.RFIDValue; + } + + this.Invalidate(); + return; + } + + + + // 텍스트 객체 찾기 + var selected_txt = mapTexts.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); + if (selected_txt != null) + { + var rlt = AR.UTIL.InputBox("input", selected_txt.Text); + if (rlt.Item1 == true) + { + selected_txt.Text = rlt.Item2; + this.Invalidate(); + } + return; + } + } + } + + protected override void OnResize(EventArgs e) + { + base.OnResize(e); + UpdateToolbarRects(); + this.Invalidate(); + } + + protected override void OnMouseWheel(MouseEventArgs e) + { + base.OnMouseWheel(e); + if (e.Delta > 0) + { + zoom *= 1.1f; + } + else + { + zoom /= 1.1f; + } + + zoom = Math.Max(0.1f, Math.Min(10.0f, zoom)); + this.Invalidate(); + } + + protected override void OnMouseDown(MouseEventArgs e) + { + base.OnMouseDown(e); + lastMousePosition = e.Location; + var mapPoint = ScreenToMap(e.Location); + + if (e.Button == MouseButtons.Middle) + { + isDragging = true; + this.Cursor = Cursors.SizeAll; + } + else if (e.Button == MouseButtons.Left && !isAddingText && !isDrawingCustomLine && !isDrawingRFIDLine) + { + isDragging = true; + + // 텍스트 선택 및 드래그 시작 + foreach (var text in mapTexts) + { + var textSize = CreateGraphics().MeasureString(text.Text, text.Font); + var rect = new RectangleF( + text.Location.X - SELECTION_DISTANCE, + text.Location.Y - SELECTION_DISTANCE, + textSize.Width + SELECTION_DISTANCE * 2, + textSize.Height + SELECTION_DISTANCE * 2 + ); + + if (rect.Contains(mapPoint)) + { + selectedText = text; + isDraggingText = true; + // 드래그 시작점과 텍스트 위치의 차이를 저장 + dragOffset = new Point( + text.Location.X - mapPoint.X, + text.Location.Y - mapPoint.Y + ); + return; + } + } + + // 커스텀 라인 선택 및 드래그 포인트 설정 + foreach (var line in customLines) + { + float startDistance = GetDistance(mapPoint, line.StartPoint); + float endDistance = GetDistance(mapPoint, line.EndPoint); + + if (startDistance < SELECTION_DISTANCE) + { + selectedLine = line; + draggingPoint = line.StartPoint; + isDraggingPoint = true; + return; + } + else if (endDistance < SELECTION_DISTANCE) + { + selectedLine = line; + draggingPoint = line.EndPoint; + isDraggingPoint = true; + return; + } + } + + + // RFID 포인트 선택 + var clickedRFID = rfidPoints.FirstOrDefault(r => + GetDistance(mapPoint, r.Location) <= SNAP_DISTANCE); + if (clickedRFID != null) + { + selectedRFID = clickedRFID; + draggingPoint = clickedRFID.Location; + isDraggingPoint = true; + return; + } + + + } + } + + protected override void OnMouseMove(MouseEventArgs e) + { + base.OnMouseMove(e); + currentMousePosition = e.Location; + var mapPoint = ScreenToMap(e.Location); + + if ((mousemode == eMouseMode.addcustomline || mousemode == eMouseMode.addrfidline) && branchPoint.HasValue) + { + UpdatePreviewLine(e.Location); + } + + // 툴바 버튼 호버 상태 업데이트 + var oldHovering = toolbarRects.OrderBy(t => t.Idx).Select(t => t.isHovering).ToArray(); + + //toolbar check + toolbarRects.ForEach(t => t.isHovering = t.Bounds.Contains(e.Location)); + + //hovering check + if (toolbarRects.Where(t => t.isHovering).Any()) + { + this.Cursor = Cursors.Hand; + } + else if (isDeletingRFIDLine) + { + this.Cursor = GetScissorsCursor(); + } + else + { + this.Cursor = Cursors.Default; + } + + //hovering display update + if (toolbarRects.Where(t => t.Dirty).Any()) + { + this.Invalidate(); + } + + if (isDragging) + { + if (e.Button == MouseButtons.Middle) + { + offset = new PointF( + offset.X + (e.Location.X - lastMousePosition.X), + offset.Y + (e.Location.Y - lastMousePosition.Y) + ); + } + else if (isDraggingText && selectedText != null) + { + // 텍스트 이동 - 드래그 오프셋 적용 + selectedText.Location = new Point( + mapPoint.X + dragOffset.X, + mapPoint.Y + dragOffset.Y + ); + } + else if (isDraggingPoint && draggingPoint.HasValue) + { + // 포인트 이동 + var delta = new Point( + mapPoint.X - ScreenToMap(lastMousePosition).X, + mapPoint.Y - ScreenToMap(lastMousePosition).Y + ); + + if (selectedLine != null) + { + // 커스텀 라인 포인트 이동 + if (draggingPoint.Value == selectedLine.StartPoint) + { + selectedLine.StartPoint = new Point( + selectedLine.StartPoint.X + delta.X, + selectedLine.StartPoint.Y + delta.Y + ); + } + else if (draggingPoint.Value == selectedLine.EndPoint) + { + selectedLine.EndPoint = new Point( + selectedLine.EndPoint.X + delta.X, + selectedLine.EndPoint.Y + delta.Y + ); + } + } + else if (selectedRFID != null) // RFID 포인트 이동 + { + // RFID 포인트 위치 업데이트 + selectedRFID.Location = new Point( + selectedRFID.Location.X + delta.X, + selectedRFID.Location.Y + delta.Y + ); + + // 연결된 RFID 라인 업데이트 + foreach (var line in rfidLines) + { + if (line.StartPoint == draggingPoint.Value) + { + line.StartPoint = selectedRFID.Location; + } + if (line.EndPoint == draggingPoint.Value) + { + line.EndPoint = selectedRFID.Location; + } + } + } + } + + lastMousePosition = e.Location; + this.Invalidate(); + } + + // 미리보기 라인 업데이트를 위한 마우스 위치 저장 + if (isDrawingRFIDLine || isDrawingCustomLine || previewStartPoint.HasValue) + { + currentMousePosition = mapPoint; + } + + this.Invalidate(); + } + + #endregion + + #region 기타 메서드 + private void UpdateToolbarRects() + { + int x, y, c, row; + var idx = 0; + + if (this.toolbarRects == null) this.toolbarRects = new List(); + else this.toolbarRects.Clear(); + + //left toolbar + x = TOOLBAR_MARGIN; + y = 10; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "+", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "-", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "1:1", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Cut", Bounds = new Rectangle(x, y, TOOLBAR_WIDTH - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + + //right toolbar + y = 10; + row = 0; + x = DisplayRectangle.Right - TOOLBAR_WIDTHR - TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Text", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Line", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Point", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Magnet", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Load", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Save", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Pos", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + y += TOOLBAR_BUTTON_HEIGHT + TOOLBAR_MARGIN; + toolbarRects.Add(new ToolBarItem { Idx = idx++, Title = "Path", Bounds = new Rectangle(x, y, TOOLBAR_WIDTHR - 2 * TOOLBAR_MARGIN, TOOLBAR_BUTTON_HEIGHT) }); + + } + + public void SetPreviewStartPoint(Point? point) + { + previewStartPoint = point; + this.Invalidate(); + } + + public void UpdatePreviewLine(Point currentPosition) + { + currentMousePosition = currentPosition; + this.Invalidate(); + } + + private Point SnapToPoint(Point point) + { + // RFID 포인트와 근접한지 확인 + foreach (var rfid in rfidPoints) + { + if (GetDistance(point, rfid.Location) <= SNAP_DISTANCE) + { + return rfid.Location; + } + } + + return point; + } + + + public RFIDPoint FindRFIDPoint(uint rfidValue) + { + if (rfidPoints == null || rfidPoints.Any() == false) return null; + return rfidPoints.FirstOrDefault(r => r.RFIDValue == rfidValue); + } + + public bool SetCurrentPosition(uint rfidValue) + { + var rfidPoint = FindRFIDPoint(rfidValue); + if (rfidPoint != null) + { + agv.CurrentPosition = rfidPoint.Location; + agv.CurrentRFID = rfidValue; + + // 목적지가 설정되어 있고 경로가 있는 경우 검증 + if (agv.TargetPosition != Point.Empty && agv.CurrentPath.Count > 0) + { + // 현재 위치가 경로에 있는지 확인 + bool isOnPath = agv.CurrentPath.Contains(agv.CurrentPosition); + + if (!isOnPath) + { + // 경로를 벗어났으므로 새로운 경로 계산 + var pathResult = CalculatePath(agv.CurrentRFID, agv.TargetRFID); + if (pathResult.Success) + { + SetCurrentPath(pathResult.Path); + } + } + + // AGV의 방향 결정 + if (agv.CurrentPath.Count > 0) + { + // 다음 목적지 찾기 + var nextPoint = agv.CurrentPath[0]; + + // 회전 가능 여부를 고려하여 방향 결정 + agv.TargetDirection = DetermineDirection(agv.CurrentPosition, nextPoint, agv.TargetPosition); + } + } + + // 목적지 RFID에 도착했고, 해당 RFID에 고정방향이 있으면 TargetDirection을 강제 설정 + if (agv.TargetRFID == rfidValue) + { + var destRFID = FindRFIDPoint(rfidValue); + if (destRFID != null && destRFID.FixedDirection.HasValue) + { + agv.TargetDirection = destRFID.FixedDirection.Value; + } + } + + this.Invalidate(); + return true; + } + return false; + } + + public bool SetTargetPosition(uint rfidValue) + { + var rfidPoint = FindRFIDPoint(rfidValue); + if (rfidPoint != null) + { + agv.TargetPosition = rfidPoint.Location; + agv.TargetRFID = rfidValue; + this.Invalidate(); + return true; + } + return false; + } + + + private List CalculatePath(Point start, Point end) + { + var openList = new List { start }; + var closedList = new List(); + var cameFrom = new Dictionary(); + var gScore = new Dictionary { { start, 0 } }; + var fScore = new Dictionary { { start, Heuristic(start, end) } }; + + while (openList.Count > 0) + { + var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First(); + + if (current == end) + { + return ReconstructPath(cameFrom, current); + } + + openList.Remove(current); + closedList.Add(current); + + foreach (var neighbor in GetNeighbors(current)) + { + if (closedList.Contains(neighbor)) + continue; + + float tentativeGScore = gScore[current] + Distance(current, neighbor); + + if (!openList.Contains(neighbor)) + openList.Add(neighbor); + else if (tentativeGScore >= gScore[neighbor]) + continue; + + cameFrom[neighbor] = current; + gScore[neighbor] = tentativeGScore; + fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor, end); + } + } + + return null; + } + private List ReconstructPath(Dictionary cameFrom, Point current) + { + var path = new List { current }; + while (cameFrom.ContainsKey(current)) + { + current = cameFrom[current]; + path.Insert(0, current); + } + return path; + } + 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 float Distance(Point a, Point b) + { + // RFID 라인을 통한 연결 확인 + var rfidLines = GetRFIDLines(); + var directConnection = rfidLines.FirstOrDefault(line => + (line.StartPoint == a && line.EndPoint == b) || + (line.StartPoint == b && line.EndPoint == a)); + + if (directConnection != null) + { + return directConnection.Distance; + } + + // 직접 연결되지 않은 경우 매우 큰 값 반환 + return float.MaxValue; + } + + private List GetNeighbors(Point point) + { + var neighbors = new List(); + var rfidLines = GetRFIDLines(); + + // RFID 라인에서 이웃 노드 찾기 + foreach (var line in rfidLines) + { + if (line.StartPoint == point) + { + neighbors.Add(line.EndPoint); + } + else if (line.EndPoint == point) + { + // 양방향 연결인 경우에만 시작점을 이웃으로 추가 + neighbors.Add(line.StartPoint); + } + } + + return neighbors.Distinct().ToList(); + } + public void SetPath(List rfids) + { + if (rfids == null || rfids.Count == 0) + return; + + var path = new List(); + foreach (var rfid in rfids) + { + var point = GetRFIDPoints().FirstOrDefault(p => p.RFIDValue == rfid); + if (point != null) + { + path.Add(point.Location); + } + } + + if (path.Count > 0) + { + SetCurrentPath(path); + } + } + + public PathResult CalculatePath(uint tagStrt, uint tagEnd) + { + var retval = new PathResult + { + Message = string.Empty, + Success = false, + Path = new List(), + }; + + var sp = tagStrt; //만약시작위치가 없다면 항상 충전기 기준으로 한다 + var ep = tagEnd; + + var startPoint = FindRFIDPoint(sp); + var endPoint = FindRFIDPoint(ep); + + if (startPoint == null || endPoint == null) + { + retval.Message = "유효한 RFID 값을 입력해주세요."; + return retval; + } + + retval.Path = CalculatePath(startPoint.Location, endPoint.Location); + if (retval.Path != null && retval.Path.Any()) + { + //SetCurrentPath(retval.Path); + + //// 경로 상의 모든 RFID 값을 가져옴 + //var rfidPath = new List(); + //foreach (var point in path) + //{ + // var rfid = GetRFIDPoints() + // .FirstOrDefault(r => r.Location == point); + // if (rfid != null) + // { + // rfidPath.Add(rfid.RFIDValue); + // } + //} + retval.Success = true; + } + else + { + retval.Message = "경로를 찾을 수 없습니다"; + } + return retval; + } private void AddNearbyRFIDPoints(RFIDLine line) { @@ -962,72 +1025,6 @@ namespace AGVControl rfidLines.Remove(line); } - private float GetProjectionRatio(Point point, Point lineStart, Point lineEnd) - { - float lineLength = GetDistance(lineStart, lineEnd); - if (lineLength == 0) return 0; - - return ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + - (point.Y - lineStart.Y) * (lineEnd.Y - lineStart.Y)) / (lineLength * lineLength); - } - - private float GetDistanceToLine(Point point, Point lineStart, Point lineEnd) - { - float lineLength = GetDistance(lineStart, lineEnd); - if (lineLength == 0) return GetDistance(point, lineStart); - - float t = ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + - (point.Y - lineStart.Y) * (lineEnd.Y - lineStart.Y)) / (lineLength * lineLength); - - t = Math.Max(0, Math.Min(1, t)); - - float projectionX = lineStart.X + t * (lineEnd.X - lineStart.X); - float projectionY = lineStart.Y + t * (lineEnd.Y - lineStart.Y); - - return GetDistance(point, new Point((int)projectionX, (int)projectionY)); - } - - protected override void OnMouseDoubleClick(MouseEventArgs e) - { - base.OnMouseDoubleClick(e); - - if (e.Button == MouseButtons.Left) - { - var mapPoint = ScreenToMap(e.Location); - - //RFID 포인트 찾기 - var selected_rfid = rfidPoints.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); - if (selected_rfid != null) - { - // 고정방향 순환 변경: 없음→전진→후진→없음 - if (!selected_rfid.FixedDirection.HasValue) - selected_rfid.FixedDirection = Direction.Forward; - else if (selected_rfid.FixedDirection == Direction.Forward) - selected_rfid.FixedDirection = Direction.Backward; - else - selected_rfid.FixedDirection = null; - - this.Invalidate(); - return; - } - - // 텍스트 객체 찾기 - var selected_txt = mapTexts.Where(t => t.Bounds.Expand(SELECTION_DISTANCE, SELECTION_DISTANCE).Contains(mapPoint)).FirstOrDefault(); - if (selected_txt != null) - { - var rlt = AR.UTIL.InputBox("input", selected_txt.Text); - if (rlt.Item1 == true) - { - selected_txt.Text = rlt.Item2; - this.Invalidate(); - } - return; - } - } - } - - - public event EventHandler OnRightClick; public void SetRFIDPoints(List points) { @@ -1128,7 +1125,6 @@ namespace AGVControl } } - public void SetIsAddingMagnet(bool value) { @@ -1177,8 +1173,6 @@ namespace AGVControl { //base.OnPaint(e); - - e.Graphics.TranslateTransform(offset.X, offset.Y); e.Graphics.ScaleTransform(zoom, zoom); @@ -1231,7 +1225,6 @@ namespace AGVControl DrawToolbarButton(e.Graphics, item.Bounds, item.Title, item.isHovering); } - private void DrawRFIDPoints(Graphics g) { // RFID 포인트 그리기 @@ -1240,7 +1233,7 @@ namespace AGVControl var MarkerSize = 5; var half = MarkerSize / 2f; rfid.Bounds = new RectangleF(rfid.Location.X - half, rfid.Location.Y - half, MarkerSize, MarkerSize); - + // 회전 가능 여부에 따라 다른 색상 사용 using (var brush = new SolidBrush(rfid.IsRotatable ? Color.Yellow : Color.Green)) { @@ -1249,9 +1242,9 @@ namespace AGVControl // 고정방향이 있으면 테두리 색상 표시 if (rfid.FixedDirection.HasValue) { - Color borderColor = rfid.FixedDirection.Value == Direction.Forward ? Color.DeepSkyBlue : Color.Gold; using (var pen = new Pen(borderColor, 2)) + Color borderColor = rfid.FixedDirection.Value == Direction.Forward ? Color.DeepSkyBlue : Color.Gold; using (var pen = new Pen(borderColor, 2)) { - g.DrawEllipse(pen, rfid.Bounds.Expand(5,5)); + g.DrawEllipse(pen, rfid.Bounds.Expand(5, 5)); } } } @@ -1306,7 +1299,7 @@ namespace AGVControl // 원 그리기 - Color bgcolor = agv.BatteryLevel > 80 ? Color.Lime : ( agv.BatteryLevel > 60 ? Color.Gold : Color.Tomato ); + Color bgcolor = agv.BatteryLevel > 80 ? Color.Lime : (agv.BatteryLevel > 60 ? Color.Gold : Color.Tomato); using (var circleBrush = new SolidBrush(Color.FromArgb(150, bgcolor))) g.FillEllipse(circleBrush, circleRect); using (var circlePen = new Pen(Color.Black, 2)) @@ -1401,8 +1394,10 @@ namespace AGVControl foreach (var connection in rfidConnections) { - var startPoint = rfidPoints.First(p => p.RFIDValue == connection.StartRFID).Location; - var endPoint = rfidPoints.First(p => p.RFIDValue == connection.EndRFID).Location; + var startPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == connection.StartRFID)?.Location ?? Point.Empty; + var endPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == connection.EndRFID)?.Location ?? Point.Empty; + + if (startPoint.IsEmpty || endPoint.IsEmpty) continue; using (Pen linePen = new Pen(Color.FromArgb(50, Color.Wheat), 2)) { @@ -1527,8 +1522,6 @@ namespace AGVControl this.Invalidate(); } - - public void ClearMap() { rfidPoints.Clear(); @@ -1552,7 +1545,6 @@ namespace AGVControl public void AddRFIDPoint(Point mapLocation, uint rfidValue) { - // 이미 맵 좌표로 변환된 위치를 사용 var rfidPoint = new RFIDPoint { Location = mapLocation, @@ -1578,8 +1570,10 @@ namespace AGVControl lines.Add("[RFID_LINES]"); foreach (var connection in rfidConnections) { - var startPoint = rfidPoints.First(p => p.RFIDValue == connection.StartRFID).Location; - var endPoint = rfidPoints.First(p => p.RFIDValue == connection.EndRFID).Location; + var startPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == connection.StartRFID)?.Location ?? Point.Empty; + var endPoint = rfidPoints.FirstOrDefault(p => p.RFIDValue == connection.EndRFID)?.Location ?? Point.Empty; + if (startPoint.IsEmpty || endPoint.IsEmpty) continue; + lines.Add($"{startPoint.X},{startPoint.Y},{endPoint.X},{endPoint.Y}," + $"{connection.StartRFID},{connection.EndRFID},{connection.IsBidirectional},{connection.Distance}"); } @@ -1902,5 +1896,179 @@ namespace AGVControl // 회전 불가능한 구간인 경우 현재 진행 방향 유지 return agv.CurrentDirection; } + + // AGV 행동 예측 함수 + public AGVActionPrediction PredictNextAction() + { + // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정) + if (agv.CurrentRFID == 0) + { + return new AGVActionPrediction + { + Direction = Direction.Backward, + NextRFID = null, + Reason = "AGV 위치 미확정(처음 기동)", + ReasonCode = AGVActionReasonCode.NoPosition + }; + } + + // 2. 경로가 없거나 현재 위치가 경로에 없음 + if (agv.CurrentPath == null || agv.CurrentPath.Count < 2 || agv.CurrentPosition == Point.Empty) + { + return new AGVActionPrediction + { + Direction = agv.CurrentDirection, + NextRFID = null, + Reason = "경로 없음 또는 현재 위치 미확정", + ReasonCode = AGVActionReasonCode.NoPath + }; + } + + // 3. 경로상에서 다음 RFID 예측 + int idx = agv.CurrentPath.FindIndex(p => p == agv.CurrentPosition); + if (idx < 0) + { + return new AGVActionPrediction + { + Direction = agv.CurrentDirection, + NextRFID = null, + Reason = "현재 위치가 경로에 없음", + ReasonCode = AGVActionReasonCode.NotOnPath + }; + } + + // 4. 목적지 도달 전, 방향 미리 판단 및 회전 위치 예측 + // 목적지 RFID 정보 + var destPoint = agv.CurrentPath.Last(); + var destRFID = rfidPoints.FirstOrDefault(r => r.Location == destPoint); + if (destRFID != null && destRFID.FixedDirection.HasValue) + { + // 목적지에 도달할 때의 방향 예측 + if (agv.CurrentPath.Count >= 2) + { + // 목적지 바로 전 위치에서 목적지로 이동할 때의 방향 + var beforeDest = agv.CurrentPath[agv.CurrentPath.Count - 2]; + Direction arriveDir = (destPoint.X > beforeDest.X) ? Direction.Forward : Direction.Backward; + if (arriveDir != destRFID.FixedDirection.Value) + { + // 목적지 도달 전, 마지막 회전 가능한 RFID를 찾음 + int lastRotatableIdx = -1; + for (int i = 0; i < agv.CurrentPath.Count - 1; i++) + { + var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[i]); + if (rfid != null && rfid.IsRotatable) + lastRotatableIdx = i; + } + if (lastRotatableIdx >= 0) + { + // 회전 가능한 위치에 도달하면 NeedTurn 반환 + if (idx == lastRotatableIdx) + { + var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx]); + return new AGVActionPrediction + { + Direction = agv.CurrentDirection, + NextRFID = rfid?.RFIDValue, + Reason = "목적지 진입방향 맞추기 위해 회전 필요", + ReasonCode = AGVActionReasonCode.NeedTurn + }; + } + else if (idx < lastRotatableIdx) + { + // 회전 가능한 위치까지 이동 안내 + var rfid = rfidPoints.FirstOrDefault(r => r.Location == agv.CurrentPath[lastRotatableIdx]); + return new AGVActionPrediction + { + Direction = (agv.CurrentPath[lastRotatableIdx].X > agv.CurrentPosition.X) ? Direction.Forward : Direction.Backward, + NextRFID = rfid?.RFIDValue, + Reason = "회전 가능한 위치로 이동 중", + ReasonCode = AGVActionReasonCode.Normal + }; + } + } + // 회전 가능한 위치가 없음 + return new AGVActionPrediction + { + Direction = agv.CurrentDirection, + NextRFID = null, + Reason = "경로상에 회전 가능한 위치가 없음", + ReasonCode = AGVActionReasonCode.Unknown + }; + } + } + } + + // 5. 목적지 도달 시(방향이 맞는 경우) + if (idx == agv.CurrentPath.Count - 1) + { + return new AGVActionPrediction + { + Direction = agv.CurrentDirection, + NextRFID = null, + Reason = "경로의 마지막 지점(목적지 도달)", + ReasonCode = AGVActionReasonCode.Arrived + }; + } + + // 6. 일반 경로 주행 + Point nextPoint = agv.CurrentPath[idx + 1]; + var nextRFID = rfidPoints.FirstOrDefault(r => r.Location == nextPoint)?.RFIDValue; + Direction nextDir = (nextPoint.X > agv.CurrentPosition.X) ? Direction.Forward : Direction.Backward; + + return new AGVActionPrediction + { + Direction = nextDir, + NextRFID = nextRFID, + Reason = null, + ReasonCode = AGVActionReasonCode.Normal + }; + } + #endregion + + #region 좌표 변환 및 계산 + 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 Point ScreenToMap(Point screenPoint) + { + int adjustedX = screenPoint.X; + return new Point( + (int)((adjustedX - offset.X) / zoom), + (int)((screenPoint.Y - offset.Y) / zoom) + ); + } + + + private float GetProjectionRatio(Point point, Point lineStart, Point lineEnd) + { + float lineLength = GetDistance(lineStart, lineEnd); + if (lineLength == 0) return 0; + + return ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + + (point.Y - lineStart.Y) * (lineEnd.Y - lineStart.Y)) / (lineLength * lineLength); + } + + + private float GetDistanceToLine(Point point, Point lineStart, Point lineEnd) + { + float lineLength = GetDistance(lineStart, lineEnd); + if (lineLength == 0) return GetDistance(point, lineStart); + + float t = ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + + (point.Y - lineStart.Y) * (lineEnd.Y - lineStart.Y)) / (lineLength * lineLength); + + t = Math.Max(0, Math.Min(1, t)); + + float projectionX = lineStart.X + t * (lineEnd.X - lineStart.X); + float projectionY = lineStart.Y + t * (lineEnd.Y - lineStart.Y); + + return GetDistance(point, new Point((int)projectionX, (int)projectionY)); + } + #endregion } } diff --git a/Cs_HMI/SubProject/AGVControl/agvControl.csproj b/Cs_HMI/SubProject/AGVControl/agvControl.csproj index 900ffa3..2a190a0 100644 --- a/Cs_HMI/SubProject/AGVControl/agvControl.csproj +++ b/Cs_HMI/SubProject/AGVControl/agvControl.csproj @@ -112,6 +112,7 @@ + \ No newline at end of file diff --git a/Cs_HMI/SubProject/AGVControl/sample.route b/Cs_HMI/SubProject/AGVControl/sample.route new file mode 100644 index 0000000..cea5fcc --- /dev/null +++ b/Cs_HMI/SubProject/AGVControl/sample.route @@ -0,0 +1,105 @@ +[RFID_POINTS] +100,486,517 +173,488,516 +230,493,515 +309,489,514 +370,490,513 +437,487,512 +483,459,511 +511,421,510 +543,371,509 +569,329,508 +608,289,507 +661,279,506 +701,297,505 +698,349,504 +698,391,503 +699,449,502 +691,491,501 +570,275,400 +517,264,401 +454,264,402 +388,262,403 +315,258,404 +639,234,600 +621,182,601 +641,183,602 +657,101,603 +627,82,604 +560,73,605 +499,65,606 +432,65,607 +264,232,405 +363,60,608 +654,508,500 +96,542,100 +159,542,101 +226,542,102 +309,541,103 +369,542,104 +483,165,753 +735,163,700 +523,170,752 +565,175,751 +597,182,750 +665,181,703 +700,176,702 +722,170,701 +[RFID_LINES] +100,486,173,488,517,516,False,73.02739 +173,488,230,493,516,515,False,57.21888 +230,493,309,489,515,514,False,79.1012 +309,489,370,490,514,513,False,61.0082 +370,490,437,487,513,512,False,67.06713 +437,487,483,459,512,511,False,53.85165 +483,459,511,421,511,510,False,47.20169 +511,421,543,371,510,509,False,59.36329 +543,371,569,329,509,508,False,49.39635 +569,329,608,289,508,507,False,55.86591 +608,289,639,234,507,600,False,63.13478 +639,234,665,181,600,703,False,59.03389 +665,181,657,101,703,603,False,80.399 +657,101,627,82,603,604,False,35.51056 +627,82,560,73,604,605,False,67.60178 +560,73,499,65,605,606,False,61.52235 +499,65,432,65,606,607,False,67 +432,65,363,60,607,608,False,69.18092 +641,183,665,181,602,703,False,24.08319 +665,181,700,176,703,702,False,35.35534 +700,176,722,170,702,701,False,22.80351 +722,170,735,163,701,700,False,14.76482 +641,183,621,182,602,601,False,20.02498 +621,182,597,182,601,750,False,24 +597,182,565,175,750,751,False,32.75668 +565,175,523,170,751,752,False,42.29657 +523,170,483,165,752,753,False,40.31129 +264,232,315,258,405,404,False,57.24509 +315,258,388,262,404,403,False,73.1095 +388,262,454,264,403,402,False,66.0303 +454,264,517,264,402,401,False,63 +517,264,570,275,401,400,False,54.12947 +570,275,608,289,400,507,False,40.49691 +608,289,661,279,507,506,False,53.93515 +661,279,701,297,506,505,False,43.86343 +701,297,698,349,505,504,False,52.08647 +698,349,698,391,504,503,False,42 +698,391,699,449,503,502,False,58.00862 +699,449,691,491,502,501,False,42.75512 +691,491,654,508,501,500,False,40.71855 +96,542,100,486,100,517,False,56.14267 +159,542,173,488,101,516,False,55.7853 +226,542,230,493,102,515,False,49.16299 +309,541,309,489,103,514,False,52 +369,542,370,490,104,513,False,52.00961 +[MAP_TEXTS] +179,251,-1,-16777216,Arial,12,OPS-2 +239,52,-256,-65408,Arial,12,SSOTRON-3 +617,527,-16711936,-16777216,Arial,12,SSOTRON-1 +87,551,-16777216,16777215,Arial,12,B1 +150,550,-16777216,16777215,Arial,12,B2 +227,553,-16777216,16777215,Arial,12,B3 +299,555,-16777216,16777215,Arial,12,B4 +367,554,-16777216,16777215,Arial,12,B5 +453,128,-16777216,-8323073,Arial,12,CHG1 +725,124,-16777216,-8323073,Arial,12,CHG2 +[CUSTOM_LINES]