はじめに

前回記事では、WR-XXからA/Dセンサー値をArduinoを使って取り出して表示するのに成功したので、今回は、ArduinoをROSノードにしてWR-XX経由で取得したセンサー値をROSでサブスクライブできるようにしてみました。

環境等

プチロボ WR-MS3L（3軸指型ロボ入門用!?）
Arduino Uno
測距センサー Sharp 2Y0A21
Ubuntu 18.04 / ROS Melodic

いろいろと調べたこと

◆ Sharp 2Y0A21の電圧データを距離数値に変更する方法

こちらは、膨大な数の人がいろいろな計算式を発表（？）しているのですが、以下の二つのサイトがよくまとまっていたので参考にさせていただきました。

Sharp/2Y0A21で測定した電圧を距離の数値化する方法としては、Arduinoとの組み合わせで説明されているものが大変多く、そのためもあってか、A/D変換を10ビットで計算するのが一般的なようですが、WR-XXのA/D変換は、8ビットで行われます。そのため、10ビット値から距離にするのではなく、測定電圧から距離に変換する計算式がありがたかったです。

シャープのGP2Y0A Series / GP2Y0D Series Application Noteには、Reciprocal distance 1/（L+0.42）(cm, L:Distance to reflective Object)という式を使って一次関数グラフに当てはめる方法も紹介されていますが、結局はグラフから一次関数式を推測することになりますので、上記のライブラリでの資料や計算式を使わせていただきました。

◆ ROSのトピックメッセージのタイプ

「ROSプログラミング」銭飛著（森北出版2016）のP.167で説明されている「超音波センサのプログラミング」を参考にして、sensor_msgs/Range messageを使用してみました。

http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Range.html
このタイプでは、超音波と赤外線の二種類のセンサーが選択できるようです。センサーの仕様に合わせて、上限値を80(cm)、下限値を10(cm)に設定しています。上限値から外れると「+Inf」が表示されますので、何に使うかは別として、発想は面白いと思いました。

なお、トピックメッセージのタイプは、sensor_msgs/LaserScanというのもあるようなのですが、これは、文字通りレーザー測位センサー用のようなので、今回は使用しませんでした。

◆ 赤外線か超音波か

実際には80センチの測定なんてとてもできず、せいぜい30センチくらいまでくらいが信頼できる数値という印象です。それに、10センチ未満は測定に適さないとなると、WR-MS3のように小さなロボットアームでは、組み合わせとしては使い道が狭いようにも思いますので、実際に動かして比べてみると、WR-MS3との組み合わせという観点では2センチまで検出できる超音波センサー（HC-SR04）の方が使い勝手が良いようにも思いました。

ソースコードと使い方

ソースコードは、前回から少し変更した部分があります。

ArduinoのsoftserialのポートをTX はD11、RXをD10に変更しました。また、「プチロボコントローラ」では、通信に抵抗を使ったレベル変換をする回路となっていますが、直接WR-XX制御基板のシリアルポートに接続しました。
制御基板に対するコマンド5での問い合わせと返信データの処理を関数にして外出しにしてみました。また、この関数では、A/D出力ピンの番号を0～4で指定することで計算結果の汎用性を高めました。

ROS関連の処理は、上述の「超音波センサのプログラミング」のセンサ仕様の処理部分以外はほぼ同じです。

2y0a21-wr-xx-ros-arduino.ino

# include <SoftwareSerial.h>
# include <ros.h>
# include <ros/time.h>
# include <sensor_msgs/Range.h>

# define BAUDRATE 38400
# define BUF         27
# define TXS         11
# define RXS         10
# define TRUE        1
# define SPLVOL      3   //sensor supply voltage

SoftwareSerial mySerial(RXS, TXS);

int buf[BUF];

ros::NodeHandle nh;

sensor_msgs::Range range_msg;
ros::Publisher pub_range("/ired_PSD", &range_msg);

char frameid[] = "/ired_PSD";

int getRange_raw(int adSoc_num)
{
    int i, j, k, n, Result;
    byte myComand[5] = {253,4,5,254};           //set Command 5 TX data
    mySerial.write(myComand,4);                 //send Command 5 TX data
    int c = mySerial.available();
    if (c > 0) {
        j = 0; k = 0;
        for (i = 0; i <= c; i++) {
            n = mySerial.read();
            if (n == 253){
                j = i; k = TRUE;      
            };
            if (k == TRUE && (i - j) >= 3 && (i - j) <= 12 ){
                buf[i - j] = n - 48;            //cnv Ascii No. to char num (0-9)
                if (buf[i - j] > 9)
                  buf[i - j] = buf[i - j] -7;   //cvt Ascii No. of A-F to dec char num (10-15)
            };
        };
        if (adSoc_num > 0)
            Result = buf[adSoc_num * 2 + 3] * 16 + buf [adSoc_num * 2 + 4];
        else 
            Result = buf[3] * 16 + buf[4];
    };
    return Result;      
}

void setup() 
{                
    Serial.begin(BAUDRATE);
    mySerial.begin(BAUDRATE);

    nh.initNode();
    nh.advertise(pub_range);

    range_msg.radiation_type = sensor_msgs::Range::INFRARED;
    range_msg.header.frame_id = frameid;
    range_msg.field_of_view = 0.1;  // fake
    range_msg.min_range = 10;
    range_msg.max_range = 80;
}

void loop() 
{
    //距離データ変換
    double Range = getRange_raw(0) * SPLVOL / 255;    //8 bit 
    double Distance = 29.988 * pow(Range, -1.173);

    range_msg.range = Distance;
    range_msg.header.stamp = nh.now();
    pub_range.publish(&range_msg);

    delay(250);

    nh.spinOnce();
}

使い方

Arduino IDEでプログラムのコンパイル、書き込みをしてから次の手順で動作を確認しました。

console

[端末1]
$ roscore &
[端末2]
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0
[端末3]
$ rostopic echo /ired_PSD

おわりに

こうしてみると、やはりどうしても、わざわざArduinoとWR-XXを組み合わせて動作させるというのはデバイス数としても、開発の労力にしても、無駄というか、なんとも納得しにくいです。センサーの分解能が8ビットというのも、本格的に（？）おもちゃ志向の商品じゃぁないか、と少しがっかりした気持ちになりました。もっとも、ロボットのセンサーというのは、情報の収集のためというよりむしろデバイスの制御のために使用するためのものなので、距離を正確に測定するのではなく三段階くらいに区別してスイッチに使用できれば十分という考え方もあると思うので、その意味では、適度な精度設計としてよく考えられている訳で、いろいろな設計思想に気づかされるのだから、この製品の教育的価値は発売から10年以上経過してもまだまだ健在ともいえるのかもしれません・・・

それとも、そのうちバージョンアップして、Arduinoの拡張ボード扱いで販売したりしても面白いかも。