Artheru：创建页面，内容为“引用LidarContoller，开发一个MainIOObject类继承于LidarDIOObject。参考以下代码获取帧并使用Output()提交点云数据。 == 1.雷达适配参数含义 == === 1.1 3D点云数据结构 === '''public''' '''struct''' '''LidarPoint3D''' { '''public''' '''float''' d;//点云距离 '''public''' '''float''' azimuth;//点云角度 '''public''' '''float''' altitude;//点云俯仰角度 '''public''' '''float''' intensity; //反…”

2025-03-10T12:26:58Z

创建页面，内容为“引用LidarContoller，开发一个MainIOObject类继承于LidarDIOObject。参考以下代码获取帧并使用Output()提交点云数据。 == 1.雷达适配参数含义 == === 1.1 3D点云数据结构 === '''public''' '''struct''' '''LidarPoint3D''' { '''public''' '''float''' d;//点云距离 '''public''' '''float''' azimuth;//点云角度 '''public''' '''float''' altitude;//点云俯仰角度 '''public''' '''float''' intensity; //反…”

新页面

引用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'''

3D激光雷达适配 - 版本历史