查看“2D激光雷达适配”的源代码

2D雷达适配时，引用LidarController，声明一个MainIOObject类，继承于Lidar2DIOObject。写一个方法读取雷达数据，当一帧读完，放入cachedLidar，调用Output()。

== 1.雷达适配参数含义 ==

=== 1.1 2D点云数据结构 ===
 '''public''' '''class''' '''LidarPoint2D'''
     {
         '''public''' '''float''' th;//点云角度
         '''public''' '''float''' d;//点云距离
         '''public''' '''float''' intensity;//物体反光度（注意，本意为reflectivity，但由于历史原因一直称为intensity了）
     }

=== 1.2 其余参数说明 ===

* fname: simulatelidar专用的，指当前点云用的是哪个文件
* 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. 2D雷达适配流程 ==
注意：适配时，雷达正前方为0°

流程示例：
 '''using''' '''LidarController''';//引用雷达父类库
 '''namespace''' '''WLR716Lidar'''//(工程名为厂商(WLR)+雷达型号(716Lidar))
 {
     '''public''' '''class''' '''MainIOObject''' : Lidar2DIOObject//类名统一为MainIOObject 继承Lidar2DIOObject
     {
         //通信变量定义
         '''private''' '''string''' _ip;
         '''private''' '''int''' _port = '''2110''';
         '''public''' '''MainIOObject'''()
         {
             //在初始化函数中填入雷达正装时对应的,扫描起始角度,结束角度,扫描方向(从产品手册或者厂家处获取)
             ScanStartAngle = -'''135''';
             ScanEndAngle = '''135''';
             ScanAngleSgn = '''1''';
         }
         //定义Loop函数用于连接设备,之后循环解析并output数据
         '''private''' '''void''' '''Loop'''()
         {
             //连接设备取流(不通设备通信方式可能不同)
             Console.'''WriteLine'''("WLR716Lidar Starting...");
             '''var''' tcpclient = '''new''' '''TcpClient'''(_ip, _port);
             '''var''' ns = tcpclient.'''GetStream'''();
 			//在while循环外定义一下四个列表用于存储解析的数据
             List<LidarPoint2D> cloud = '''new''' List<LidarPoint2D>();
             List<'''float'''> thetaList = '''new''' List<'''float'''>();
             List<'''int'''> distList = '''new''' List<'''int'''>();
             List<'''float'''> intensityList = '''new''' List<'''float'''>();
 			//pck数组用于存读取的原始报文
             '''byte'''[] pck = '''new''' '''byte'''['''1024'''];
             
             DateTime ticStart = DateTime.Now;//扫描一帧点云开始时间
             Console.'''WriteLine'''($"Start streaming on port {_port}");
            
             '''while''' ('''true''')
             {
 
                 '''int''' len = '''898''';
                 '''int''' n = ns.'''Read'''(pck, '''0''', len);//读取报文到pck中
                 '''if''' (len != '''ToInt32Rev'''(pck, '''4''') + '''9''')//校验报文长度
                     Console.'''WriteLine'''("pck len does not match");
 
                 '''var''' thisScanC = '''ToInt32Rev'''(pck, '''46''');//读取当前帧
                 '''if''' (scanC != thisScanC)//如果上一帧读取完成(例如读取到的帧号不同,判断方法可以不通),赋值cachedLidar,并output
                 {
                     scanC = thisScanC;//赋值scanC
                     '''if''' (thetaList.Count=='''0''') '''continue''';
                     //将存储的角度,距离,反光度信息转换为点云列表
                     '''for''' ('''int''' i = '''0'''; i < thetaList.Count; ++i)
                     {
                         cloud.'''Add'''('''new''' LidarPoint2D
                         {
                             th = (mirror ? -thetaList[i] : thetaList[i]),//倒转角度取反
                             d = distList[i],
                             intensity = intensityList[i] / ReflexRange // 反光度归一化,(如何设备返回的是回波强度,不是反光度,需要转换为反广度,例如乘以距离,具体转换效果需要实测,观察点云可视化,通过调整ReflexRange需要让反光板处点云明显比别处点云偏红)
                         });
                     }
                     cachedLidar = cloud.'''Where'''(pt => pt.th >= angleStart && pt.th <= angleEnd && pt.d>minDist && pt.d<maxDist).'''ToArray'''();//将th in [angleStart,angleEnd],d in [minDist,maxDist] 范围内的点云赋值给cachedlidar
                     maxReflex = cachedLidar.'''Select'''(p=>p.intensity).'''Max'''();//更新当前帧的最大反光度
                     frame += '''1''';//frame累加一次
                     interval = (DateTime.Now - ticStart).TotalMilliseconds;//更新扫描一帧点云的时间
                     ticStart = DateTime.Now;//重置开始时间
                     tick = DateTime.Now.Ticks;//更新当前tick
                     '''output'''();//上传数据
                     //清空四个列表
                     cloud.'''Clear'''();
                     thetaList.'''Clear'''();
                     distList.'''Clear'''();
                     intensityList.'''Clear'''();
                 }
 				//解析pck中的数据填写到thetaList,distList,intensityList中
                 '''FillData'''();//根据不通雷达的报文协议进行解析
                 //如果原始雷达定义的角度0°方向不在雷达正前方,需要将读到的点云角度加上一个固定的角度,使0°方向在雷达正前方,然后加入thetaList中
             }
         }
 		//定义启动函数
         '''public''' '''void''' '''Start'''('''string''' ip,'''int''' port = '''2110''')
         {
             Console.'''WriteLine'''($"WLR716Lidar on {ip}");
             '''this'''._ip = ip;
             //新建线程进行连接读取,如果读取异常,重新执行Loop函数
             '''new''' '''Thread'''(() =>
             {
                 Console.'''WriteLine'''($"Try WLR716Lidar");
                 '''while''' ('''true''')
                 {
                     '''try'''
                     {
                         ConnectTimes+='''1''';//连接次数+1
                         '''Loop'''();
                     }
                     '''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并实例化相应的对象赋值给等号左侧的lidar
 lidar=io load plugins/WLR716Lidar.dll
 //Start对应Start函数，传的参数放在后面，空格隔开。如WLR716雷达中Start方法有ip和port两个参数，所以需要传两个参数
 lidar Start '''192.168'''.'''0.2''' '''2110'''