/// <summary>
/// 计算获取靶标
/// </summary>
/// <param name="poaler"></param>
/// <param name="angle"></param>
public void GetBaBiao(int[] poaler, float [] angle, AGV agvtem)
{
this.agv = agvtem;
Polar = new double[poaler.Length];
Angle = new double[poaler.Length];
Array.Copy(poaler, 0, Polar, 0, poaler.Length);
Array.Copy(angle, 0, Angle, 0, angle.Length);
ComputeMag();
}
/// <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());
}
}
public CurrentWorld(GraphAevironment g, AGV car)
{
this.agv = car;
this.G = g;
}
public SingleAgv(AGV agvtem, int swyox)
{
this.agv = agvtem;
this.xoydirction = swyox;
SwitchXoY();
}
/// <summary>
/// AGV在全局坐标系下坐标 /反光柱与AGV的距离角度
/// </summary>
/// <param name="datalisttem"></param>反光柱与AGV的距离和角度
/// <param name="agvpointem"></param>AGV在全局坐标系下坐标
public SingleAgv(AGV agvtem)
{
this.agv = agvtem;
xoydirction = 0;
SwitchXoY();
}
public AgvAbsoluteLocation(ref AGV agvv)
{
this.agv = agvv;
CalcuteAverage();
}