2D激光雷达适配

来自MDCS wiki
跳到导航 跳到搜索

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
取自“https://wiki.lessokaji.com/index.php?title=2D激光雷达适配&oldid=881”