3D激光雷达适配
跳到导航
跳到搜索
引用LidarContoller，开发一个MainIOObject类继承于LidarDIOObject。参考以下代码获取帧并使用Output()提交点云数据。
1.雷达适配参数含义
1.1 3D点云数据结构
public struct LidarPoint3D
{
public float d;//点云距离
public float azimuth;//点云角度
public float altitude;//点云俯仰角度
public float intensity; //反光度（实际上本意指reflectivity，历史原因用错了名称，但为了兼容性目前不做修改）
public float progression;//点云在帧中的时间顺序在哪
}
1.2 其余参数说明
- ScanAngleSgn: 雷达硬件属性--扫描方向 1为逆时针，-1为顺时针 对应detour雷达配置中的angleSgn
- ScanStartAngle: 雷达硬件属性--一帧扫描起始角度（-180°~+180°）
- ScanEndAngle: 雷达硬件属性--一帧扫描结束角度（-180°~+180°）对应detour雷达配置中的endAngle
- angleStart: 过滤点云角度用 最小角度
- angleEnd: 过滤点云角度用 最大角度
- maxDist: 过滤点云距离用 最大距离
- minDist: 过滤点云距离用 最小距离
- ReflexRange: 归一化尺度,从雷达解析的反光度需要除以该值
- cachedLidar :缓存用于output的帧数据
- frame: output一帧后加一 output 的帧数量
- tick: output前更新当前tick
- scanC: 雷达提供的自上电起帧数量
- maxReflex: 当前一帧数据中最强的反光率
- pointsN: 一帧数据的点数
- mirror: 是否倒装 倒装为true
2.3D雷达适配流程
注意：适配时，雷达正前方为0°
namespace LeishenC16_v4_0_8
{
public class MainIOObject : Lidar3DIOObject
{
private string _targetIp;//雷达IP
private const int MsopPort = 2368;//雷达点云数据发送端口
private const int DifopPort = 2369;//雷达设备数据发送端口
public MainIOObject()
{
//雷达硬件参数
ScanAngleSgn = -1;
ScanStartAngle = 180;
ScanEndAngle = -180;
}
[IOObjectMonitor] private int _motSpd; //rpm
private int _returnMode; // 55: strongest
// 56: last
// 57: dual
[IOObjectMonitor] public long CachedTick = 0;
[IOObjectMonitor] public double ScanInterval; // ms
[IOObjectMonitor] public double RealInterval; // ms
[IOObjectUtility]
public void Stuck()
{
Task.Factory.StartNew(() =>
{
var tic = DateTime.Now;
while (tic.AddSeconds(5) > DateTime.Now)
{
CachedTick += (long)(Math.Sin(tic.Ticks) * 100);
}
});
}
public static float ThDiff(float th1, float th2)
{
return (float)(th1 - th2 -
Math.Round((th1 - th2) / 360.0f) * 360);
}
//16线雷达每条线俯仰角度
private readonly float[] _altitudeTable = new float[]
{
-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15
};
private long _startHour;
private void Loop(int port1, int port2)
{
// MSOP
Console.WriteLine("RSLidar16 MSOP Starting...");
var msoPclient = new UdpClient(port1);//绑定本地接受端口
// msoPclient.Client.Bind(new IPEndPoint(IPAddress.Any, port1));//绑定本地端口?
var sendEndPoint = new IPEndPoint(IPAddress.Any, 0);//用于获取发送端的ip 和 端口 不用指定port
Console.WriteLine($"RSLidar16 starts streaming on port {MsopPort}");
// DIFOP
Console.WriteLine("LeishenC16 DIFOP Starting...");
var difoPclient = new UdpClient(port2);//绑定本地接受端口
// difoPclient.Client.Bind(new IPEndPoint(IPAddress.Any, port2));
var difoPsendEndPoint = new IPEndPoint(IPAddress.Any, 0);
//获取设备信息
void GetDeviceInfo()
{
while (true)
{
//读取数据包（difop）
var pck = difoPclient.Receive(ref difoPsendEndPoint);
//判断数据包是否来自雷达
if (difoPsendEndPoint.Address.ToString() != _targetIp)
{
Console.WriteLine("Wrong IP! Trying agian...");
continue;
}
_motSpd = pck[8] * 256 + pck[9];
//计算扫描周期
ScanInterval = 60f / _motSpd * 1000;
break;
}
}
var typicalLen = 0;
GetDeviceInfo();
long ticLastSent = 0;
long ticLast = 0;
var lastFrame = new List<LidarPoint3D>();
long ticNew = 0;
var newFrame = new List<LidarPoint3D>();
long lidarTicLast = -1;
long pTime = 0;
while (true)
{
//读取数据包（msop）
var pck = msoPclient.Receive(ref sendEndPoint);
if (sendEndPoint.Address.ToString() != _targetIp || pck.Length != 1212)//校验发送方IP,和报文长度
{
Console.WriteLine("Invalid UDP package received! from ip:{sendEndPoint.Address.ToString()} pckl:{pck.Length}");
continue;
}
if (pck[1210] == 57)//按照单回波方式解析的报文,只支持雷达端设置单回波模式
{
Console.WriteLine($"* Return mode not single return!");
throw new Exception("Only single return supported");
}
//pck 1200y,1201m,1202d,1203h,1204m,1205s
var second = new DateTime(pck[1200] + 1, pck[1201], pck[1202], pck[1203], pck[1204], pck[1205]).Ticks /
TimeSpan.TicksPerSecond;
var nanosecond = BitConverter.ToInt32(pck, 1206);
pTime = second * 1000000 + nanosecond / 1000;//计算当前包的时间戳
var idx = 0;
float delta = 0;
for (var i = 0; i < 12; ++i)
{
idx += 2;
List<LidarPoint3D> target;
var azimuth = (pck[idx + 1] * 256 + pck[idx]) * 0.01f;//按照报文解析方位角
if (idx + 100 < 1200)
{
var aNext = (pck[idx + 100 + 1] * 256 + pck[idx + 100]) * 0.01f;
delta = ThDiff(aNext, azimuth);
}
idx += 2;
//判断是否有效
bool DetermineFrame()
{
target = lastFrame;
var eFrameSt = pTime - (long)(azimuth / 360 * ScanInterval * 1000);
if (ticLast == 0)
{
ticLast = eFrameSt;
return true;
}
if (ticLast - ScanInterval * 1000 * 0.1 < eFrameSt &&
eFrameSt < ticLast + ScanInterval * 1000 * 0.1) return true; // previous frame.
if (eFrameSt > ticLast + ScanInterval * 1000 * 1.3)
{
ticLast = eFrameSt;
target.Clear();
newFrame.Clear();
return false;
}
target = newFrame;
if (eFrameSt < ticLast - ScanInterval * 1000 * 0.3)
return false; // even older frame.
if (eFrameSt > ticLast + ScanInterval * 1000 * 0.3)
{
// new frame.
ticNew = eFrameSt;
}
return true;
}
void Read()
{
for (var j = 0; j < 16; ++j)
{
var d = pck[idx + 1] * 256 + pck[idx];
var refl = pck[idx + 2];
idx += 3;
target.Add(new LidarPoint3D()
{
azimuth = ThDiff(mirror ? azimuth - 180 : -(azimuth - 180),0),//减去180度旋转0度方向
altitude = mirror ? -_altitudeTable[j] : _altitudeTable[j],
d = d * 4f,
intensity = refl / 256f,
progression = (float)((azimuth - Math.Floor(azimuth / 360) * 360) / 360)
});
}
}
if (!DetermineFrame()) continue;
Read();
//由于两个俯仰角度的点云只给出一个角度信息，所以另一个需要根据相邻两包的数据进行差值计算
azimuth += delta / 2;
if (azimuth > 360) azimuth -= 360;
if (!DetermineFrame()) continue;
Read();
}
//计算时间，若大于一个扫描周期，则进行output
if (pTime - ticLast > ScanInterval * 1.3 * 1000)
{
var scanCnt = 1;
if (ticLastSent != 0 && ticLast - ticLastSent > ScanInterval * 1000 * 1.3f)
scanCnt = (int)Math.Round((ticLast - ticLastSent) / (ScanInterval * 1000));
scanC += scanCnt;
frame += 1;
cachedLidar = lastFrame.ToArray();
if (frame < 10)
typicalLen = Math.Max(typicalLen, cachedLidar.Length);
else if (typicalLen * 0.8 < cachedLidar.Length)
Output();
else
Console.WriteLine($"bad length or corrupted packet, only {cachedLidar.Length} points");
RealInterval = (ticLast - ticLastSent) / 1000d;
lastFrame = newFrame;
newFrame = new List<LidarPoint3D>();
ticLastSent = ticLast;
ticLast = ticNew;
//每10帧读取一次difop
if (scanC % 10 == 0) new Thread(GetDeviceInfo).Start();
}
}
}
public void Start(string ip, int port1, int port2)
{
Console.WriteLine($"LeishenC16 on {ip}");
this._targetIp = ip;
new Thread(() =>
{
Console.WriteLine($"Try LeishenC16");
while (true)
{
try
{
//设置is3D属性为true，代表是3D雷达
is3D = true;
Loop(port1, port2);
}
catch (Exception ex)
{
Console.WriteLine($"msg:{ex.Message}, stack:{ex.StackTrace}");
Thread.Sleep(1000);
}
}
}).Start();
}
}
}
生成插件后，需在Medulla的startup.iocmd中增加以下代码：
//io load xxx.dll时，会寻找dll里面class MainIOObject并实例化相应的对象赋值给等号左侧的frontlidar3d frontlidar3d = io load plugins/LeishenC16_v4_0_8.dll //Start对应Start函数，传的参数放在后面，空格隔开。 frontlidar3d Start 192.168.0.2 2368 2369