查看“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'''