OpenRCFでLiDAR LD19の点群を読む

本稿では，LiDAR「LD19」の点群データを読み取り，OpenRCFのシミュレータに表示する方法について説明します．

CRC8を使えるようにする．

「SerialDevice.cs」ファイル内の「SerialDevice」クラスの中に「CheckCode」クラスがある．「CheckCode」クラスの中に以下の配列と関数を追加する．

private static byte[] tableCRC8 = new byte[256]
{
    0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34,0xe3, 
    0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, 
    0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, 
    0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, 
    0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
    0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, 
    0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, 
    0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, 
    0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, 
    0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, 
    0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, 
    0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, 
    0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, 
    0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, 
    0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, 
    0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, 
    0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, 
    0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, 
    0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, 
    0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, 
    0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, 
    0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8, 
};

public static byte GetCRC8(byte[] bytes, int startIndex, int endIndex)
{
    byte result = 0; 

    for (int i = startIndex; i <= endIndex; i++)
    {
        result = tableCRC8[(result ^ bytes[i]) & 0xFF];
    }

    return result;
}

LD19クラスを追加する．

「SerialDevice.cs」ファイル内の「SerialDevice」クラスの中に以下のLD19クラスを追加する．

public class LD19 : SerialBase
{
    private ReceiveDataHandler RecvDataHdlr = new ReceiveDataHandler();
    private static Header Header = new Header(0x54);

    public LD19(int baudrate = 230400)
    {
        SetBaudRate(baudrate);
        SerialPort.Parity = Parity.None;
        SerialPort.Handshake = Handshake.None;
        SerialPort.DataReceived += DataReceiveEvent;
    }

    private void DataReceiveEvent(object sender, SerialDataReceivedEventArgs e)
    {
        byte[] readBytes = SerialRead();
        RecvDataHdlr.Read(readBytes);                
    }

    public float[][] PointCloud { get { return RecvDataHdlr.ReceivedPointCloud; } }

    public byte[] Intensities { get { return RecvDataHdlr.ReceivedIntensities; } }

    public void ConsoleWritePointCloud(int startNum = 224, int endNum = 227)
    {
        float[][] pointCloud = PointCloud;

        if (startNum < pointCloud.GetLength(0) && endNum < pointCloud.GetLength(0))
        {
            for (int i = startNum; i < endNum; i++)
            {
                Console.WriteLine(String.Join(" ", pointCloud[i]));
            }

            Console.WriteLine();
        }
    }

    private class ReceiveDataHandler
    {
        private Packet ReadPacket = new Packet();

        private float stepAngle = 0.8f * (float)Math.PI / 180f;
        private static int pointNum = 36000 / 80;
        private ushort[] distance = new ushort[pointNum];
        public byte[] intensity = new byte[pointNum];

        public float[][] ReceivedPointCloud
        {
            get
            {
                float[][] result = new float[pointNum][];
                float theta;

                for (int i = 0; i < result.GetLength(0); i++)
                {
                    theta = stepAngle * i;
                    result[i] = new float[3];
                    result[i][0] = -0.001f * distance[i] * (float)Math.Sin(theta);
                    result[i][1] = -0.001f * distance[i] * (float)Math.Cos(theta);
                }

                return result;
            }
        }

        public byte[] ReceivedIntensities
        {
            get
            {
                byte[] result = new byte[pointNum];
                for (int i = 0; i < result.GetLength(0); i++) { result[i] = intensity[i]; }
                return result;
            }
        }

        public void Read(byte[] readBytes)
        {
            ReadPacket.Stack = readBytes;                                       
                    
            while (Header.Length < ReadPacket.Length)
            {
                if (Header.IsMatch(ReadPacket.Get))
                {
                    byte lower5Bits = (byte)(ReadPacket[Header.Length] & 0x1F);
                    int length = 3 * lower5Bits + 11;
                            
                    if (ReadPacket.Length < length) return;

                    byte crc = CheckCode.GetCRC8(ReadPacket.Get, 0, length - 2);

                    if (crc == ReadPacket[length - 1]) 
                    {
                        ushort speed = ReadPacket.ToUInt16(2);
                        ushort startAngleDeg100 = ReadPacket.ToUInt16(4);
                        int i = startAngleDeg100 / 80;

                        for (int j = 6; j < 6 + 3 * 12; j += 3)
                        {
                            if (i < pointNum)
                            {
                                distance[i] = ReadPacket.ToUInt16(j);
                                intensity[i] = ReadPacket[j + 2];
                                i++;
                            }
                            else
                            {
                                break;
                            }
                        }
                                
                        ReadPacket.CutOut(length);
                    }
                    else
                    {                                
                        Console.WriteLine("Error : CheckCode (CRC) does not match.");
                        ReadPacket.Reset();
                    }
                }
                else
                {                            
                    Console.WriteLine("Error : Header does not match.");
                    ReadPacket.CutOutUntil(Header.Bytes);
                }
            }
            
        }
    }
}

MainWindow.xaml.csでの使用例

SerialDevice.LD19 LD19 = new SerialDevice.LD19();
RoughSphere Point = new RoughSphere(0.015f);
float[][] pointCloud = new float[0][];

void Loop()
{
    pointCloud = LD19.PointCloud;
}

void Draw()
{  
    for (int i = 0; i < pointCloud.GetLength(0); i++)
    {
        Point.Position = pointCloud[i];
        Point.Draw();
    }
}

void Button1_Click(object sender, RoutedEventArgs e)
{
    LD19.PortOpen("COM7");
}

ボタン1を押すとLiDAR（LD19）と接続され，点群がシミュレータ上に表示されます．COMの番号はデバイスマネージャーで確認し，適宜修正してください．
LD19.Intensities（byte型の配列）には，それぞれの点の反射強度が記録されています．

補足：デバイスが認識されない場合

デバイスマネージャーで確認しても，COM番号が割り当てられていない場合は，下記のサイトからドライバーをインストールすると治ります．
https://wiki.youyeetoo.com/en/Lidar/D300
「Download」→「USB Driver」と進み，「CP210x～」を入手する．
デバイスマネージャーでCP210x～を右クリックし，「コンピューターを参照してドライバーを検索」を押し，先ほどのドライバーを選択する．