/// <summary> /// 加载反光柱的图层 /// </summary> /// <returns></returns> public static GraphAevironment LoadTheLightMsg() { GraphAevironment graph = null; try { string[] read = File.ReadAllLines(Application.StartupPath + @"\TheLightInformation.ini"); graph = new GraphAevironment(read.Length); double [,] TEM = new double[read.Length, read.Length]; for (int i = 0; i < read.Length; i++) { TheLightNode node = JsonConvert.DeserializeObject <TheLightNode>(read [i]); graph.SetNode(node.NumberNo - 1, node); //添加节点 } for (int J = 0; J < graph.NODE.Length; J++) { for (int K = J; K < graph.NODE.Length; K++) { if (J != K) { double len = Math.Sqrt(Math.Pow(graph.NODE[J].Polar, 2) + Math.Pow(graph.NODE [K].Polar, 2) - 2 * graph.NODE[J].Polar * graph.NODE[K].Polar * Math.Cos(Math.PI * (graph.NODE[J].Angle - graph.NODE[K].Angle) / 180)); graph.SetMatrix(J, K, len); //添加边 } } } TEM = graph.MATRIX; } catch (Exception ex) { MessageBox.Show(ex.ToString()); } return(graph); }
void ComputeMag() { if (LidarMode.GraphCreateState) { for (int i = 0; i < Polar.Length; i++) { TheLightNode node = new TheLightNode(); if (Polar [i] != 0 && Polar [i] != 0) { node.X = Polar [i] * Math.Cos(Math.PI * Angle [i] / 180); node.Y = Polar [i] * Math.Sin(Math.PI * Angle [i] / 180); node.NumberNo = (byte)(nodelist.Count + 1); node.Angle = Angle [i]; node.Polar = Polar [i]; nodelist.Add(node); } } } else { if (LidarMode.IsNavicationMode) { for (int l = 0; l < agv.IsBatter.Count; l++) { if (agv.IsBatter[l][1] != 0 && agv.IsBatter [l][2] != 0) { TheLightNode node = new TheLightNode(); node.NumberNo = (byte )agv.IsBatter [l][0]; node.X = agv.IsBatter [l][1]; node.Y = agv.IsBatter [l][2]; node.Angle = agv.IsBatter [l][3]; node.Polar = agv.IsBatter [l][4]; nodelist.Add(node); } } } if (LidarMode.TargetDetectionState) { for (int j = 0; j < agv.NotBatter.Count; j++) { TheLightNode node = new TheLightNode(); node.NumberNo = (byte )agv.NotBatter [j][0]; node.X = agv.NotBatter [j][1]; node.Y = agv.NotBatter [j][2]; node.Angle = agv.NotBatter [j][3]; node.Polar = agv.NotBatter [j][4]; nodelist.Add(node); } } } }
/// <summary> /// /// </summary> /// <param name="edge"></param> /// <returns></returns> public TheLightNode[] GetCurrentWorldCoordinate(double [,] edge) { TheLightNode [] wordxy = new TheLightNode[edge.GetLength(0)]; int k = 0; for (int i = 0; i < edge.GetLength(0); i++) { List <TheLightNode> nodelist = new List <TheLightNode> (); for (int j = 0; j < edge.GetLength(1); j++) { if (i != j && edge [i, j] != 0) { TheLightNode[] node = new TheLightNode [2]; node [0] = new TheLightNode(); node [1] = new TheLightNode(); node = GetEdgeNode(edge [i, j]); if (node != null) { nodelist.Add(node[0]); nodelist.Add(node [1]); } } } var res = from n in nodelist group n by n into g orderby g.Count() descending select g; if (nodelist.Count >= 4) { var gr = res.First(); foreach (TheLightNode itenn in gr) { wordxy[k] = new TheLightNode(); wordxy [k] = itenn; k++; break; } } else { wordxy[k] = new TheLightNode(); wordxy [k] = null; k++; } } return(wordxy); }
/// <summary> /// 删除边 /// </summary> /// <param name="node1"></param> /// <param name="node2"></param> public void DeleEdge(TheLightNode node1, TheLightNode node2) { if (!IsNode(node1) || !IsNode(node2)) { MessageBox.Show("没有加入此节点反光柱!"); return; } else { Matrix [GetNodeIndex(node1), GetNodeIndex(node2)] = 0; Matrix [GetNodeIndex(node2), GetNodeIndex(node1)] = 0; --numEdge; } }
/// <summary> /// 添加边 /// </summary> /// <param name="node1"></param> /// <param name="node2"></param> /// <param name="value"></param> public void SetEdge(TheLightNode node1, TheLightNode node2, double value) { if (!IsNode(node1) || !IsNode(node2)) { MessageBox.Show("没有加入此节点反光柱!"); return; } else { Matrix [GetNodeIndex(node1), GetNodeIndex(node2)] = value; Matrix [GetNodeIndex(node2), GetNodeIndex(node1)] = value; ++numEdge; } }
/// <summary> /// 判断是否是其节点 /// </summary> /// <param name="node"></param> /// <returns></returns> public bool IsNode(TheLightNode node) { foreach (TheLightNode item in Node) { if (-Para.XYERROR <= (item.X - node.X) && (item.X - node.X) <= Para.XYERROR) { if (-Para.XYERROR <= (item.Y - node.Y) && (item.Y - node.Y) <= Para.XYERROR) { return(true); } } } return(false); }
/// <summary> /// 获取节点的编号 /// </summary> /// <param name="node"></param> /// <returns></returns> public int GetNodeIndex(TheLightNode node) { int indexx = -1; for (int i = 0; i < Node.Length; i++) { if (-Para.XYERROR <= (Node[i].X - node.X) && (Node[i].X - node.X) <= Para.XYERROR) { if (-Para.XYERROR <= (Node[i].Y - node.Y) && (Node[i].Y - node.Y) <= Para.XYERROR) { indexx = i; } } } return(indexx); }
/// <summary> /// 根据两个反光柱之间的距离提取两个反光柱的坐标信息 /// </summary> /// <param name="value"></param> /// <returns></returns> public TheLightNode [] GetEdgeNode(double value) { TheLightNode[] node = new TheLightNode[2]; node [0] = new TheLightNode(); node [1] = new TheLightNode(); for (int i = 0; i < Matrix.GetLength(0); i++) { for (int j = 0; j < Matrix.GetLength(1); j++) { if (-Para.LENERROR <= (Matrix [i, j] - value) && (Matrix [i, j] - value) <= Para.LENERROR) { node [0] = GetNode(i); node [1] = GetNode(j); } } } return(node); }
/// <summary> /// 判断两个反关柱之间是否添加边 /// </summary> /// <param name="node1"></param> /// <param name="node2"></param> /// <returns></returns> public bool IsExistEdge(TheLightNode node1, TheLightNode node2) { if (!IsNode(node1) || !IsNode(node2)) { MessageBox.Show("没有加入此节点反光柱!"); return(false); } else { if (Matrix [GetNodeIndex(node1), GetNodeIndex(node2)] == 0) { return(false); } else { return(true); } } }
/// < /// summary> /// /// </summary> void GetAGVLocation() { if (agv.Polar != null && agv.Angle != null) { int [] poalte = agv.Polar.ToList().Distinct().ToArray(); float [] antem = agv.Angle.ToList().Distinct().ToArray(); double [,] edge = new double[poalte.Length, poalte.Length]; TheLightNode [] worldcoord = new TheLightNode[poalte.Length]; for (int i = 0; i < poalte.Length; i++) { worldcoord [i] = new TheLightNode(); } edge = GetEdge(poalte, antem); worldcoord = G.GetCurrentWorldCoordinate(edge); GetBatter(worldcoord, poalte, antem); AgvAbsoluteLocation agvlocation = new AgvAbsoluteLocation(ref agv); if (LidarMode.TargetDetectionState) //靶标探测模式 { SingleAgv addbiaoba = new SingleAgv(agv); } } GC.Collect(); }
/// <summary> /// 获取靶标 /// </summary> /// <param name="agv"></param> public void GetBaBiao(AGV agv) { try { if (agv != null) { this.agv = agv; if (agv.Polar != null) { Polar = new double[agv.Polar.Length]; Angle = new double[agv.Angle.Length]; Array.Copy(agv.Polar, 0, Polar, 0, agv.Polar.Length); Array.Copy(agv.Angle, 0, Angle, 0, agv.Angle.Length); if (LidarMode.GraphCreateState) { for (int i = 0; i < Polar.Length; i++) { TheLightNode node = new TheLightNode(); if (Polar [i] != 0 && Polar [i] != 0) { node.X = Polar [i] * Math.Cos(Math.PI * Angle [i] / 180); node.Y = Polar [i] * Math.Sin(Math.PI * Angle [i] / 180); node.NumberNo = (byte)(nodelist.Count + 1); node.Angle = Angle [i]; node.Polar = Polar [i]; nodelist.Add(node); } } } else { if (LidarMode.IsNavicationMode) { List <double []> isbarrerlist = new List <double[]> (); if (isbarrerlist.Count == 0) { isbarrerlist.AddRange(agv.DisIsBatter); } agv.DisIsBatter.Clear(); for (int i = 0; i < isbarrerlist.Count; i++) { TheLightNode node = new TheLightNode(); if (isbarrerlist != null) { if (isbarrerlist [i][0] != 0 && isbarrerlist [i][1] != 0) { node.X = isbarrerlist [i][1]; node.Y = isbarrerlist [i][2]; node.NumberNo = (byte )isbarrerlist [i][0]; node.Angle = isbarrerlist [i][3]; node.Polar = isbarrerlist [i][4]; isnodelist.Add(node); } } } } if (LidarMode.TargetDetectionState) { List <double []> isbarrerlist = new List <double[]> (); if (isbarrerlist.Count == 0) { isbarrerlist.AddRange(agv.DisIsBatter); } for (int i = 0; i < isbarrerlist.Count; i++) { TheLightNode node = new TheLightNode(); if (isbarrerlist != null) { if (isbarrerlist [i][0] != 0 && isbarrerlist [i][1] != 0) { node.X = isbarrerlist [i][1]; node.Y = isbarrerlist [i][2]; node.NumberNo = (byte )isbarrerlist [i][0]; node.Angle = isbarrerlist [i][3]; node.Polar = isbarrerlist [i][4]; isnodelist.Add(node); } } } List <double []> notbarrerlist = new List <double[]> (); if (notnodelist.Count == 0) { notbarrerlist.AddRange(agv.NotBatter); } for (int i = 0; i < notbarrerlist.Count; i++) { TheLightNode node = new TheLightNode(); if (notbarrerlist != null && notbarrerlist [i][0] != 0 && notbarrerlist [i][1] != 0) { node.X = notbarrerlist [i][1]; node.Y = notbarrerlist [i][2]; node.NumberNo = (byte )notbarrerlist [i][0]; node.Angle = notbarrerlist [i][3]; node.Polar = notbarrerlist [i][4]; notnodelist.Add(node); } } agv.DisIsBatter.Clear(); agv.NotBatter.Clear(); } } } } } catch (Exception ex) { MessageBox.Show(ex.ToString()); } }
/// <summary> /// 设置定点信息 /// </summary> /// <param name="index"></param> /// <param name="node"></param> public void SetNode(int index, TheLightNode node) { Node [index] = node; }