UDP通信によるCE 30のデータ収集

33055 ワード

1.CE 30の紹介

CE 30−DはTOF原理に基づいて開発した固体面アレイレーザレーダである.単線機械回転レーダと比較して、機械回転部品が一切ないため、長期にわたって安定で信頼性の高い運転が可能であり、より広い垂直検出範囲を得ることができる.TOF原理:レーザーエミッタで光パルスを発し、物体に遭遇すると光が反射し、レンズは捕捉した光線、すなわちその飛行時間によって物体とレンズの間の距離を判断する.光源、レンズ、感光素子の3つの部分が主に含まれています.

2.udp初期化プロセス

ce 30との通信では、まず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;
}

上記のコードは、recvfrom関数およびsendto関数を用いて受信および送信のプロセスを達成するGetPacketおよびSendPacket関数をカプセル化する.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により取得コマンドのコマンドをce 30に送信する

diagnose = socket.GetPacket(version_response);

GetPacketでce 30から返されたバイト情報を受信し、バージョン番号情報がtrueに正常に返されたと判断し、失敗して「Get Version'not Responding」に戻る

5.フレーム（320*20）のデータを取得する

パケットフォーマット:CE 30-Dは列ごとにデータを送信する形式を採用し、式は320列のデータを27個のパケットに分解して送信し、各パケットはCE 30-Dの12列のデータを送信し、そのうち上位26個のパケットには12列の真実測距データが含まれ、第27個のパケットには8列の真実データと4列の充填列が含まれている.各列のデータは横方向の偏向角に対応し、各行のデータは垂直角度に対応する.各フレームのデータは、データヘッダ、データブロック、タイムスタンプ、出荷情報に分けられ、各パケットは合計816バイトを含む.各パケットには、0～41の42バイトのデータヘッダが含まれます.②64バイトのデータブロック12個;③4バイトのタイムスタンプ;④1つの2バイトの出荷情報;ここで、各データブロックは、感光アレイ内の上から下までの縦列全体のすべての感光要素情報を収容する.各データブロックは、0 xFFEEの値を持つ1つの2バイトの識別コードを含む.②2バイトの横方向偏射角;③20個の感光体情報;ここで、各感光要素情報は、1個の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.パケットの取得および解析

CE 30 D出力の距離と角度は極座標形式であり、3 D点群画像の再構築と応用を容易にするために、直角座標系に変換することができ、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(42 byte)+12列*(ColumnIdentifiierBytes(2 byte)+AzimuthBytes(2 byte)+20(pixel)*(distance(2 byte)+amp(1 byte))+TimeStampBytes(4 byte)+FactoryBytes(2 byte)
呼び出し関数:HeaderBytes,ColumnIdentifiierBytes,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.偏射角ソルバ

横方向バイアス角は2つのデータで表される.0 x 18 0 x 06の横偏向角情報を受信すると、その解算ステップ:①2バイト分のデータを入れ替える順番:0 x 06 0 x 18②2 2バイト整数を得る:0 x 618③10進数に変換する:1560④100で割る角度:15.60°

8.感光メタデータの解算

感光メタデータは3バイトで構成され、距離と強度の2つの情報が含まれている.距離は2バイト、強度は1バイトです.0 x 85 0 x 26 0 x 00の感光性メタデータを受け取り、その算出方式は以下の通りである:1前の2バイトのデータを入れ替える順序:0 x 26 0 x 85②2バイトの整数型を得る:0 x 2685③10進数に変換する:9861④2.0 mmを乗じる:19722 mm⑤1000で割る距離:19.722 m

9.タイムスタンプソルバ

タイムスタンプは、最初のデータブロックが取得した時間に対応します.4バイトで構成されています.例えば、0 x 61 0 x 67 0 x B 9 0 x 5 Aのタイムスタンプデータが受信され、その算出方式は以下の通りである:①4バイトデータを反転する順序:0 x 5 A 0 x B 9 0 x 67 0 x 61②4バイト整数を得る:0 x 5 AB 96761③10進数に変換:152100065④100000で除算して秒数を得る:152.100065

スピンロック

C言語標準ライブラリの概要[2]-文字判定テスト