private static void addRadarPoint()
{
// 线段序列为空
if (proSeg == null)
{
return;
}
// 数据不满足要求，则返回
KeyPoint p = PortManager.drPort.getPosition();
UrgModel um = PortManager.urgPort.getUrgData();
if (um.Distance.Count < UrgInfo.ang180Index)
{
return;
}
// 货物激光雷达位置信息
KeyPoint kp = CarInfo.getRadarPosition(p);
um.Distance[UrgInfo.ang000Index] = (um.Distance[UrgInfo.ang000Index] < 100) ? UrgInfo.maxDist : um.Distance[UrgInfo.ang000Index];
um.Distance[UrgInfo.ang180Index] = (um.Distance[UrgInfo.ang180Index] < 100) ? UrgInfo.maxDist : um.Distance[UrgInfo.ang180Index];
// 添加线段
proSeg.addPoint(
new SimPoint()
{
x = kp.x + Math.Cos(p.w + Math.PI) * um.Distance[UrgInfo.ang180Index] / 1000,
y = kp.y + Math.Sin(p.w + Math.PI) * um.Distance[UrgInfo.ang180Index] / 1000
},
new SimPoint()
{
x = kp.x + Math.Cos(p.w) * um.Distance[UrgInfo.ang000Index] / 1000,
y = kp.y + Math.Sin(p.w) * um.Distance[UrgInfo.ang000Index] / 1000
});
}
/////////////////////////////////////////////// private attribute ////////////////////////////////////////////
private bool getUrgPoint(UrgPort urgPort)
{
config.pointsH = new List <URG_POINT>();
if (!urgPort.IsOpen)
{
return(false);
}
UrgModel urgModel = urgPort.getUrgData();
if (urgModel.Distance == null)
{
return(false);
}
int BG = (int)((75 - urgPort.AngleStart) / urgPort.AnglePace);
int ED = (int)((105 - urgPort.AngleStart) / urgPort.AnglePace);
if (urgModel.Distance.Count < ED)
{
return(false);
}
config.pointsH = new List <URG_POINT>();
for (int i = BG; i < ED; i++)
{
if (urgModel.Distance[i] == 0)
{
continue;
}
URG_POINT point = new URG_POINT();
point.d = urgModel.Distance[i];
double angle = urgPort.AngleStart + i * urgPort.AnglePace;
point.a = angle;
point.x = point.d * Math.Cos(angle * Math.PI / 180);
point.y = point.d * Math.Sin(angle * Math.PI / 180);
config.pointsH.Add(point);
}
return(true);
}
private List <CONFIG.URG_POINT> getUrgPoint(double angleBG, double angleED)
{
List <CONFIG.URG_POINT> points = new List <CONFIG.URG_POINT>();
if (!PortManager.urgPort.IsOpen)
{
return(points);
}
UrgModel urgModel = PortManager.urgPort.getUrgData();
while (urgModel.Distance == null || urgModel.Distance.Count == 0)
{
urgModel = PortManager.urgPort.getUrgData();
}
int BG = (int)((angleBG - -30) / (360.0 / 1024.0));
int ED = (int)((angleED - -30) / (360.0 / 1024.0));
if (urgModel.Distance.Count < ED)
{
return(points);
}
for (int i = BG; i < ED; i++)
{
if (urgModel.Distance[i] == 0)
{
continue;
}
CONFIG.URG_POINT point = new CONFIG.URG_POINT();
point.d = urgModel.Distance[i];
double angle = -30.0 + i * 360.0 / 1024.0;
point.a = angle;
point.x = point.d * Math.Cos(angle * Math.PI / 180);
point.y = point.d * Math.Sin(angle * Math.PI / 180);
points.Add(point);
}
return(points);
}
public UrgModel getUrgData()
{
while (config.IsSetting)
{
;
}
config.IsGetting = true;
List <long> Distance = new List <long>();
foreach (long idis in config.Distance)
{
Distance.Add(idis);
}
config.IsGetting = false;
UrgModel urgModel = new UrgModel();
urgModel.Distance = Distance;
return(urgModel);
}
