public void Init()
{
Lidar_Ini.Read_ini_Cfg3();
LidarFunc.Lidar_Accuracy = 1 / Lidar_Ini.Lidar_Accuracy;
//Lidar_Accuracy = 1 / Lidar_Ini.Lidar_Accuracy;
LidarFunc.Lidar_Segmentation = LidarFunc.Lidar_Accuracy * 360;
LidarBuffer = new byte[1248];
VerticalAngle = new double[32];
CosVerticalAngle = new double[32];
SinVerticalAngle = new double[32];
rLidarData = new LidarData();
rLidarData.X = new float[23040];
rLidarData.Y = new float[23040];
rLidarData.Z = new float[23040];
/*rLidarData.X = new float[(int)(LidarFunc.Lidar_Segmentation * 32)];
* rLidarData.Y = new float[(int)(LidarFunc.Lidar_Segmentation * 32)];
* rLidarData.Z = new float[(int)(LidarFunc.Lidar_Segmentation * 32)];*/
r_LidarData = new LidarData();
r_LidarData.X = new float[11520];
r_LidarData.Y = new float[11520];
r_LidarData.Z = new float[11520];
/*r_LidarData.X = new float[(int)((LidarFunc.Lidar_Segmentation * 32) / 2)];
* r_LidarData.Y = new float[(int)((LidarFunc.Lidar_Segmentation * 32) / 2)];
* r_LidarData.Z = new float[(int)((LidarFunc.Lidar_Segmentation * 32) / 2)];*/
}
public int ObstacleDetected(LidarData Data, DetectedRectRegion AlarmRegion, DetectedRectRegion BanRegion, out ObstacleData AlarmObData, out ObstacleData BanObData)
{
int Result = 0;
AlarmObData = new ObstacleData();
BanObData = new ObstacleData();
double Distance = 0;
for (int index = 0; index < Data.X.Length; index++)
{
if (AlarmRegion.DetectRegionX1 < Data.X[index] && Data.X[index] < AlarmRegion.DetectRegionX2 && AlarmRegion.DetectRegionY1 < Data.Y[index] && Data.Y[index] < AlarmRegion.DetectRegionY2 && AlarmRegion.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < AlarmRegion.DetectRegionZ2)
{
Distance = Math.Sqrt(Data.X[index] * Data.X[index] + Data.Y[index] * Data.Y[index] + Data.Z[index] * Data.Z[index]);
if (Data.X[index] != 0 && Data.Y[index] != 0 && Data.Z[index] != 0)
{
if (Data.Z[index] >= Obstacle_Ini.Lidar_Obstacle_Height)
{
if (BanRegion.DetectRegionX1 < Data.X[index] && Data.X[index] < BanRegion.DetectRegionX2 && BanRegion.DetectRegionY1 < Data.Y[index] && Data.Y[index] < BanRegion.DetectRegionY2 && BanRegion.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < BanRegion.DetectRegionZ2)
{
BanObData.ObstaclePosX.Add(Data.X[index]);
BanObData.ObstaclePosY.Add(Data.Y[index]);
BanObData.ObstaclePosZ.Add(Data.Z[index]);
BanObData.ObstacleDistance.Add(Distance);
}
else
{
AlarmObData.ObstaclePosX.Add(Data.X[index]);
AlarmObData.ObstaclePosY.Add(Data.Y[index]);
AlarmObData.ObstaclePosZ.Add(Data.Z[index]);
AlarmObData.ObstacleDistance.Add(Distance);
}
}
}
}
}
if (BanObData.ObstaclePosX.Count != 0)
{
Result = 2;
}
else if (AlarmObData.ObstaclePosX.Count != 0)
{
Result = 1;
}
//回傳0 為無障礙物
//回傳1 為警告區域內有障礙物
//回傳2 為禁止區域有障礙物
return(Result);
}
public bool ObstacleDetected(LidarData Data, DetectedRectRegion Region, out ObstacleData ObData)
{
bool Isdetect = false;
ObData = new ObstacleData();
double Distance = 0;
for (int index = 0; index < Data.X.Length; index += 100)
{
if (Region.DetectRegionX1 < Data.X[index] && Data.X[index] < Region.DetectRegionX2 && Region.DetectRegionY1 < Data.Y[index] && Data.Y[index] < Region.DetectRegionY2 && Region.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < Region.DetectRegionZ2)
{
Distance = Math.Sqrt(Data.X[index] * Data.X[index] + Data.Y[index] * Data.Y[index] + Data.Z[index] * Data.Z[index]);
Isdetect = true;
ObData.ObstaclePosX.Add(Data.X[index]);
ObData.ObstaclePosY.Add(Data.Y[index]);
ObData.ObstaclePosZ.Add(Data.Z[index]);
ObData.ObstacleDistance.Add(Distance);
}
}
return(Isdetect);
}
public int CircleObstacleDetected(LidarData Data, DetectCircleRegion WarningCircleRegion, DetectCircleRegion AlarmCircleRegion, out ObstacleData WarningObData, out ObstacleData AlarmObData)
{
//bool Isdetect = false;
int Result = 0;
WarningObData = new ObstacleData();
AlarmObData = new ObstacleData();
double Distance = 0;
double radius = 0;
for (int index = 0; index < Data.X.Length; index++)
{
radius = Math.Sqrt((Data.X[index] - WarningCircleRegion.CenterX) * (Data.X[index] - WarningCircleRegion.CenterX) + (Data.Y[index] - WarningCircleRegion.CenterY) * (Data.Y[index] - WarningCircleRegion.CenterY));
if (Data.X[index] != 0 && Data.Y[index] != 0 && Data.Z[index] != 0)
{
if (WarningCircleRegion.ZDown <= Data.Z[index] && Data.Z[index] <= WarningCircleRegion.ZUp && radius <= WarningCircleRegion.Radius)
{
if (Data.Z[index] >= Obstacle_Ini.Lidar_Obstacle_Height)
{
//if (BanRegion.DetectRegionX1 < Data.X[index] && Data.X[index] < BanRegion.DetectRegionX2 && BanRegion.DetectRegionY1 < Data.Y[index] && Data.Y[index] < BanRegion.DetectRegionY2 && BanRegion.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < BanRegion.DetectRegionZ2)
if (radius < WarningCircleRegion.Radius && radius < AlarmCircleRegion.Radius && radius > 1)
{
//當在警戒範圍內時(降速)
AlarmObData.ObstaclePosX.Add(Data.X[index]);
AlarmObData.ObstaclePosY.Add(Data.Y[index]);
AlarmObData.ObstaclePosZ.Add(Data.Z[index]);
AlarmObData.ObstacleDistance.Add(Distance);
}
else if (radius < WarningCircleRegion.Radius && radius > AlarmCircleRegion.Radius && radius > 1)
{
//當在危險範圍內時(停止)
WarningObData.ObstaclePosX.Add(Data.X[index]);
WarningObData.ObstaclePosY.Add(Data.Y[index]);
WarningObData.ObstaclePosZ.Add(Data.Z[index]);
WarningObData.ObstacleDistance.Add(Distance);
}
/*Distance = Math.Sqrt(Data.X[index] * Data.X[index] + Data.Y[index] * Data.Y[index] + Data.Z[index] * Data.Z[index]);
* Isdetect = true;
* ObData.ObstaclePosX.Add(Data.X[index]);
* ObData.ObstaclePosY.Add(Data.Y[index]);
* ObData.ObstaclePosZ.Add(Data.Z[index]);
* ObData.ObstacleDistance.Add(Distance);*/
}
}
}
}
if (AlarmObData.ObstaclePosX.Count != 0)
{
Result = 2;
}
else if (WarningObData.ObstaclePosX.Count != 0)
{
Result = 1;
}
return(Result);
//return Isdetect;
}
public void Init()
{
LidarBuffer = new byte[1248];
VerticalAngle = new double[32];
CosVerticalAngle = new double[32];
SinVerticalAngle = new double[32];
rLidarData = new LidarData();
rLidarData.X = new float[23040];
rLidarData.Y = new float[23040];
rLidarData.Z = new float[23040];
r_LidarData = new LidarData();
r_LidarData.X = new float[11520];
r_LidarData.Y = new float[11520];
r_LidarData.Z = new float[11520];
}
