センサとの接続と距離データの取得

センサから距離データを取得する基本的な手順は

センサに対して接続を行う。
- シリアルポート名を指定して接続する。(シリアル、USB 接続のとき)
- IP アドレスを指定して接続する。(Ethernet 接続のとき)

データ受信のための領域を確保する
- malloc などで領域を確保する。

センサから距離データを取得する。
- 計測を開始する。
- 計測された距離データを受信する。

センサとの接続を閉じる。
- 計測を中断し、センサとの接続を切断する。

となります。

上記の処理をプログラムで記述したものを、以下に示します。

// シリアル接続でのセンサとの接続と距離データの取得
#include "urg_sensor.h"
#include "urg_utils.h"
#include <stdlib.h>
int main(void)
{
    urg_t urg;
    int ret;
    long *length_data;
    int length_data_size;
    // "COM1" は、センサが認識されているデバイス名にする必要がある
    const char connect_device[] = "COM1";
    const long connect_baudrate = 115200;
    // センサに対して接続を行う。
    ret = urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate);
    if (ret < 0) {
        // todo: check error code
        return 1;
    }
    // データ受信のための領域を確保する
    length_data = (long *)malloc(sizeof(long) * urg_max_data_size(&urg));
    // todo: check length_data is not NULL
    // センサから距離データを取得する。
    ret = urg_start_measurement(&urg, URG_DISTANCE, 1, 0);
    if (ret < 0) {
        // todo: check error code
        return 1;
    }
    length_data_size = urg_get_distance(&urg, length_data, NULL);
    if (length_data_size <= 0) {
        // todo: check error code
        return 1;
    }
    // todo: process length_data array
    // センサとの接続を閉じる。
    urg_close(&urg);
    return 0;
}

プログラム中に出てくる urg_t 構造体は urg_open() 関数を呼び出したときに初期化され、センサパラメータの取得や以降のデータ送受信を管理しています。

このプログラムを実行すると length_data 領域に length_data_size 個のデータが格納されます。
取得した距離データを順に表示するには、以下のように記述します。

for (i = 0; i < length_data; ++i) {
    printf("%ld, ", length_data[i]);
}
printf("\n");

また、センサとの接続をイーサーネットで行う場合 urg_open() の記述を以下のように変更します。

    // イーサーネット接続でのセンサとの接続と距離データの取得
    const char connect_address[] = "192.168.0.10";
    const long connect_port = 10940;
    // センサに対して接続を行う。
    ret = urg_open(&urg, URG_ETHERNET, connect_address, connect_port);
    if (ret < 0) {
        // todo: check error code

連続して距離データを取得する

scan_times 回の距離データを取得するプログラムは、以下のように記述できます。

    // urg_start_measurement() 関数でスキャン回数を指定し
    // urg_get_distance() 関数で指定した回数だけデータを受信する。
    const int scan_times = 123;
    int length_data_size;
    int i;
    // センサから距離データを取得する。
    ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, 0);
    if (ret < 0) {
        // \todo check error code
        return 1;
    }

大抵の場合 urg_start_measurement() と urg_get_distance() のペアを繰り返して呼び出すよりも、取得回数を指定して１度 urg_start_measurement() を呼び出しておき urg_get_distance() で繰り返しデータを取得する方が効率よくデータを取得できます。

計測のパラメータ設定

計測の範囲、ステップ数の間引き設定、スキャンの間引き設定を行うことができます。計測の範囲を狭くしたり、間引きを行うことで受信データ量を減らすことができ、ライブラリ動作の負荷が減少します。

    // 計測パラメータの設定
    // センサに対して接続を行う。
    // 接続を行うと、計測パラメータの設定は初期化される
    ret = urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate);
    // \todo check error code
    // 計測範囲を指定する
    // センサ正面方向の 90 [deg] 範囲のデータ取得を行い、ステップ間引きを行わない例
    first_step = urg_rad2step(&urg, -45);
    last_step = urg_rad2step(&urg, +45);
    skip_step = 0;
    ret = urg_set_scanning_parameter(&urg, first_step, last_step, skip_step);
    // \todo check error code
    // 計測回数と計測の間引きを指定して、計測を開始する
    // 123 回の計測を指示し、スキャンの間引きを行わない例
    scan_times = 123;
    skip_scan = 0;
    ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, skip_scan);

距離データを取得したときのタイムスタンプを取得する

計測がいつのタイミングで行われたかは urg_get_distance() 関数で取得できます。

urg_get_distance() を用いて取得したタイムスタンプを表示するプログラムは、以下のように記述できます。

    // タイムスタンプの取得
    // urg_get_distance() 関数に変数を与え、タイムスタンプを取得する。
    const int scan_times = 123;
    int length_data_size;
    long timestamp;
    int i;
    // センサから距離データを取得する。
    ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, 0);
    if (ret < 0) {
        // \todo check error code
        return 1;
    }

通常はセンサの計測周期で計測が行われますが、ライブラリを処理する PC の負荷が高い場合は計測データのタイムスタンプ間隔が、センサの計測周期よりも長くなる場合があります。この場合、計測するステップ数を間引くなどの変更で対処できることがあります。

距離データを X-Y 座標系に変換する

計測した距離データを X-Y 座標に変換するには urg_index2rad() を用いてその距離がどの方向にあるかを計算します。

// 距離データを X-Y 座標系に変換して表示する
length_data_size = urg_get_distance(&urg, length_data, NULL);
for (i = 0; i < length_data_size; ++i) {
    // その距離データのラジアン角度を求め、X, Y の座標値を計算する
    double radian;
    long length;
    long x;
    long y;
    radian = urg_index2rad(&urg, i);
    length = length_data[i];
    // \todo check length is valid
    x = (long)(length * cos(radian));
    y = (long)(length * sin(radian));
    printf("(%ld, %ld), ", x, y);
}
printf("\n");

urg_index2rad() は、センサ正面が 0 [deg] としたときのステップの向きをラジアン単位で返します。

センサと urg_index2rad() の関係