距離データの取得

概要

ここでは、URG センサから距離データを取得する方法を説明します。
URG から距離データを取得する場合の基本的な流れは

• URG との接続を開く。
• URG に距離データを要求する。
• URG から距離データを取得し、判定処理などを行う。

となります。

サンプルコード

以下のサンプルでは Scan_times で指定した回数だけ距離データを取得し、

• 取得できた距離データの個数
• タイムスタンプ
• 正面方向の距離データ

を出力しています。

/*
  複数回のデータ取得
*/

#include <memory>
#include <iostream>
#include "Lidar.h"
#include "open_lidar_device.h"
#include "log_printf.h"

using namespace hrk;
using namespace std;


int main(int argc, char *argv[])
{
    auto_ptr<Lidar> lidar(open_lidar_device(argc, argv));
    if (!lidar->is_open()) {
        cerr << FILE_POSITION() << lidar->what() << endl;
        return 1;
    }

    // 計測開始
    enum { Scan_times = 10 };
    if (!lidar->start_measurement(Lidar::Distance, Scan_times)) {
        cerr << FILE_POSITION() << lidar->what() << endl;
        return 1;
    }

    vector<long> data;
    long timestamp;
    int front_index = lidar->deg2index(0);
    for (int i = 0; i < Scan_times; ++i) {
        // 計測したデータの取得
        if (!lidar->get_distance(data, &timestamp)) {
            cerr << FILE_POSITION() << lidar->what() << endl;
            return 1;
        }

        // 計測したデータの表示
        size_t n = data.size();
        cout << "n = " << n
             << ", timestamp = " << timestamp
             << ", front_data = " << data[front_index]
             << endl;
    }

    return 0;
}

実行結果の例

$ ./multi_scan
n = 726, timestamp = 16333, front_data = 1776
n = 726, timestamp = 16433, front_data = 1782
n = 726, timestamp = 16533, front_data = 1775
n = 726, timestamp = 16632, front_data = 1775
n = 726, timestamp = 16732, front_data = 1774
n = 726, timestamp = 16832, front_data = 1775
n = 726, timestamp = 16932, front_data = 1777
n = 726, timestamp = 17032, front_data = 1776
n = 726, timestamp = 17132, front_data = 1780
n = 726, timestamp = 17232, front_data = 1777