用UDP通信对CE30进行数据的采集

1.CE30的介绍

CE30-D是基于TOF原理开发的固态面阵激光雷达。与单线机械旋转雷达相比，没有任何的机械旋转部件，因此可以长期稳定、可靠的运行，并且可以得到更广的垂直探测范围。
TOF原理：利用激光发射器发出光脉冲，遇到物体后，光线反射，镜头通过捕捉的光线即其飞行的时间来判断物体和镜头之间的距离。它主要包含三个部分:光源、镜头和感光元件。

2.udp初始化过程

在与ce30通讯下需要首先配置UDP，绑定其IP和端口号。

 setsockopt(
    gWinUDPSocket, SOL_SOCKET, SO_RCVTIMEO,
    (const char*)&timeout, sizeof(timeout));
    struct sockaddr_in address;
    address.sin_family = AF_INET;
    address.sin_addr.s_addr = INADDR_ANY;
    address.sin_port = htons(port_);
    gDeviceSocketAddress.sin_family = AF_INET;
    gDeviceSocketAddress.sin_addr.s_addr = inet_addr(ip_.c_str());
    gDeviceSocketAddress.sin_port = htons(port_);
    if (bind(gWinUDPSocket, (struct sockaddr*)&address, sizeof(address)) == SOCKET_ERROR)
    {
        return Diagnose::connect_failed;
    }
    return Diagnose::connect_successful;
}

3.接收数据和发送数据

Diagnose UDPSocket::GetPacket(PacketBase &pkt, const double time_offset)
{
    int length;
    struct sockaddr_in address;
    if ((length =
                recvfrom(
                    gWinUDPSocket, gUDPSocketReadBuffer, gReadBufferLength, 0,
                    (struct sockaddr*)&address, &gSockAddrInLength)) ==
            SOCKET_ERROR)
    {
        return Diagnose::receive_error;
    }
    if (length < pkt.data.size())
    {
        return Diagnose::receive_error;
    }
    memcpy(pkt.data.data(), gUDPSocketReadBuffer, pkt.data.size());
    return Diagnose::receive_successful;
}

Diagnose UDPSocket::SendPacket(const PacketBase& packet)
{
    memcpy(gUDPSocketReadBuffer, packet.data.data(), packet.data.size());
    if (sendto(
                gWinUDPSocket, gUDPSocketReadBuffer, packet.data.size(), 0,
                (struct sockaddr*)&gDeviceSocketAddress, gSockAddrInLength) ==
            SOCKET_ERROR)
    {
        return Diagnose::send_fail;
    }
    return Diagnose::send_successful;
}

上述代码通过对GetPacket和SendPacket函数进行封装，其内容通过使用recvfrom函数和sendto函数达到接收和发送的过程。recvfrom的返回值（These calls return the number of bytes received，or -1 if an error occurred），成功返回接收的字节数，失败返回-1.通过对返回值的判断，使用memcpy函数将接收到的数据从源内存地址的起始位置拷贝到目标内存地址中。

4.发送获取版本号、更改IP、启动测试、停止测试

auto diagnose = socket.SendPacket(version_request);

通过sendPacket将获取命令的指令发送给ce30

diagnose = socket.GetPacket(version_response);

通过GetPacket接收由ce30返回的字节信息，通过判断版本号信息成功返回true，失败返回“‘Get Version’ not Responding”

5.获取一帧（320*20）数据

数据包格式：CE30-D采用逐列发送数据的形式，式，将320列数据拆解为27个数据包发送，每个数据包将发送CE30-D的12列数据，其中前26 个数据包中包含12列真实测距数据，第27 个数据包中包含8列真实数据和4列填充列。 每列数据对应一个横向偏射角，每行数据对应一个垂直角度。每帧数据可分为数据头、数据块、时间戳和出厂信息，每个数据包共计包含 816字节。
每个数据包包含：
① 一个42 字节的数据头，值为0到41；
② 12个64字节的数据块；
③ 一个4字节的时间戳；
④ 一个2字节的出厂信息；
其中，每个数据块容纳感光阵列中由上至下一整个纵列的所有感光元信息。每个数据块包含：
① 一个2字节的识别码，其值为0xFFEE；
② 一个2字节的横向偏射角；
③ 20个感光元信息；
其中，每个感光元信息包含
① 一个2字节的距离信息；
② 一个1字节的强度信息；

shared_ptr<PointCloud> cloud(new PointCloud);
    cloud->points.reserve(scan.Width() * scan.Height());
    if (scan.Ready())
    {
        for (int x = 0; x < scan.Width(); ++x)
        {
            for (int y = 0; y < scan.Height(); ++y)
            {
                Point p = scan.at(x, y).point();
                if (sqrt(p.x * p.x + p.y * p.y) < 0.01f)
                {
                    continue;
                }
                cloud->points.push_back(p);
            }
        }
    }

6.获取数据包以及解析数据包

CE30D输出的距离和角度是极坐标形式，为了便于重建3D点云图像和应用，可通过如下公式转化为直角坐 标系，X、Y、Z分别表示直角坐标系的坐标
X = Dist * sin（90 - V）* cos（H - 30）
Y = Dist * sin（90 - V）* sin（H - 30）
Z = Dist * sin（90 - V）
其中：
Dist：该感光元输出的距离值
V：Vertical为该感光元的垂直角度，范围是 -1.9° ~ +1.9°
H：Hoeizontal为该感光元的水平角度，范围是 0° ~ +60°

极坐标生成欧式坐标：

Point Channel::point() const
{
    return
        Point(
            distance * sin(ToRad(90.0f - v_azimuth)) * cos(ToRad(h_azimuth)),
            distance * sin(ToRad(90.0f - v_azimuth)) * sin(ToRad(h_azimuth)),
            distance * cos(ToRad(90.0f - v_azimuth)));
}

数据包格式：HeaderBytes(42byte) + 12列 * (ColumnIdentifierBytes(2byte)+AzimuthBytes(2byte) +20(pixel) * (distance(2byte) + amp(1byte)) ) +TimeStampBytes(4byte) + FactoryBytes(2byte)

调用函数：HeaderBytes，ColumnIdentifierBytes，AzimuthBytes，DistanceBytes…

Packet::Packet()
{
    data.resize(
        HeaderBytes() +
        ParsedPacket::ColumnNum() * (
            ColumnIdentifierBytes() +
            AzimuthBytes() +
            Column::ChannelNum() * (
                DistanceBytes() +
                AmplitudeBytes())) +
        TimeStampBytes() +
        FactoryBytes(),
        0);
}

解析Packet，20*12列数据放到ParsedPacket里面，并返回ParsedPacket的指针

std::unique_ptr<ParsedPacket> Packet::Parse()
{
    std::unique_ptr<ParsedPacket> null_packet;
    std::unique_ptr<ParsedPacket> parsed_packet(new ParsedPacket);
    parsed_packet->grey_image = IsGreyImage(data[GreyImageStatusIndex()]);
    int index = HeaderBytes();
    for (auto& col : parsed_packet->columns)
    {
        if (data[index++] != ColumnIdentifierHigh())
        {
            return null_packet;
        }
        if (data[index++] != ColumnIdentifierLow())
        {
            return null_packet;
        }
        auto azimuth_low = data[index++];
        auto azimuth_high = data[index++];
        col.azimuth = ParseAzimuth(azimuth_high, azimuth_low);
        int chn_index = 0;
        for (auto& chn : col.channels)
        {
            auto dist_low = data[index++];
            auto dist_high = data[index++];
            auto amp = data[index++];

//      cout << hex << (short)dist_low << " " << (short)dist_high << endl;

            if (parsed_packet->grey_image)
            {
                chn.grey_value = ParseGreyValue(dist_high, dist_low);
            }
            else
            {
                chn.distance = ParseDistance(dist_high, dist_low);
            }
            chn.amplitude = ParseAmplitude(amp);
            chn.amp_raw = amp;

//      cout << chn.distance << endl;

            chn.h_azimuth = col.azimuth - Scan::FoV() / 2.0f;
            chn.v_azimuth = Scan::LookUpVerticalAzimuth(chn_index++);

//      cout << chn.h_azimuth << " " << chn.v_azimuth << endl;
        }
    }
    vector<unsigned char> stamp_raw(4, 0);
    for (auto& i : stamp_raw)
    {
        i = data[index++];
    }
    std::reverse(stamp_raw.begin(), stamp_raw.end());
    parsed_packet->time_stamp = ParseTimeStamp(stamp_raw);
    return parsed_packet;
}

7.偏射角解算

横向偏射角由两个数据表示。若收到0x18 0x06的横向偏射角信息，其解算步骤：
① 调换两个字节数据的顺序：0x06 0x18
② 获得两字节整型：0x618
③ 转换成十进制：1560
④ 除以100得到角度：15.60°

8.感光元数据解算

感光元数据由3个字节构成，包含了距离和强度两种信息。其中距离占2个字节，强度占1个字节。
收到0x85 0x26 0x00的感光元数据，其解算方式如下：
解算距离：
① 调换前两个字节数据的顺序：0x26 0x85
② 获得两字节整型：0x2685
③ 转换成十进制：9861
④ 乘以2.0毫米：19722毫米
⑤ 除以1000得到距离：19.722米

9.时间戳解算

时间戳对应了第一个数据块获得的时间。由4字节构成。
。例如收到了0x61 0x67 0xB9 0x5A 的时间戳数据，
其解算方式如下：
① 翻转四个字节数据的顺序：0x5A 0xB9 0x67 0x61
② 获得四字节整型：0x5AB96761
③ 转换成十进制：1522100065
④ 除以1000000获得秒数：1522.100065