ROSで32bit整数型の配列をメッセージとして使用する

ROSにおけるメッセージ

ROSではトピックへPublish/Subscribeするメッセージの中身(データ形式)は開発者自身が決められるなんとも便利な仕様となっております。例えば、ROSの公式サイトのチュートリアルなんかではPublisherノードの宣言として下記のようなコードが紹介されてます。

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

これはchatter_pubというPublisherノードがchatterというトピックにString型のメッセージを配信するよということです。
ROSの公式サイトにはこれが日本語で丁寧に説明されていてとてもわかりやすかったです。しかし、説明はString型の使い方のみ…。僕は諸事情で32bit整数型の配列をメッセージとして使いたかったのですが、日本語で詳しく解説しているwebサイトは見当たらず…(そんな甘くないか笑)。ということで、サンプルコードを拾ったので、実際に使用してみた感じを書いてみます。

そもそもたくさんあるメッセージのヘッダファイル

ROSにはメッセージに使用するためにデータ形式を定義しますが、その種類も豊富です。ヘッダファイルの場所は(Ubuntuの場合)下記の通りです。

/opt/ros/ROS version/include/std_msgs

ヘッダファイルの中にはメンバの定義とかがたくさん書いています。

Int32MultiArray型のメッセージを配信するPublisher

配列データの型名はInt32MultiArrayです。
なにはともあれまずその参考にしたサンプルコードをご覧ください。

Publish.cpp

#include <stdio.h>
#include <stdlib.h>

#include "ros/ros.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"

#include "std_msgs/Int32MultiArray.h"

int main(int argc, char **argv)
{


    ros::init(argc, argv, "arrayPublisher");

    ros::NodeHandle n;

    ros::Publisher pub = n.advertise<std_msgs::Int32MultiArray>("array", 100);

    while (ros::ok())
    {
        std_msgs::Int32MultiArray array;
        //Clear array
        array.data.clear();
        //for loop, pushing data in the size of the array
        for (int i = 0; i < 90; i++)
        {
            //assign array a random number between 0 and 255.
            array.data.push_back(rand() % 255);
        }
        //Publish array
        pub.publish(array);
        //Let the world know
        ROS_INFO("I published something!");
        //Do this.
        ros::spinOnce();
        //Added a delay so not to spam
        sleep(2);
    }

}

解説

#include "ros/ros.h"

これはROSを使用するならば基本的にインクルードするヘッダファイルですね。
Int32MultiArrayを使用したい場合、次が肝心です。

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"

まず上の2つのヘッダファイルは配列を使用したい場合に宣言します。配列のデータ形式も数種類あって、Int8、Int16、Int32などなど様々あります。そこで具体的にどのデータ形式を使うかによって3つ目のヘッダファイルをインクルードするわけです。

    ros::init(argc, argv, "arrayPublisher");

    ros::NodeHandle n;

    ros::Publisher pub = n.advertise<std_msgs::Int32MultiArray>("array", 100);

ros::initはノードとしての名前をつけて初期化します。ros::Publisherクラスによるpubという
オブジェクトを宣言します。このオブジェクトでは、
「"array"というトピックに向けてInt32MultiArray型のメッセージをPublishする」ということになります。

        std_msgs::Int32MultiArray array;
        //Clear array
        array.data.clear();
        //for loop, pushing data in the size of the array
        for (int i = 0; i < 90; i++)
        {
            //assign array a random number between 0 and 255.
            array.data.push_back(rand() % 255);
        }

Int32MultiArray型のオブジェクトを宣言し、array.data.clear( )関数によって配列の中身を初期化します。
array.data.push_back(rand( ) % 255)によって実際にデータをプッシュします。
その後の処理に関してはROSのチュートリアルに書いてあるような一般的なPublisherと同じですね。

Int32MultiArray型のメッセージを購読するSubscriber

Subscriberに関してはあまり難しくありません。というか、ROSのチュートリアルとそんなに変わらないです。

解説

Subscribe.cpp

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <iostream>

#include "ros/ros.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"

int Arr[90];
void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array);

int main(int argc, char **argv)
{

    ros::init(argc, argv, "arraySubscriber");

    ros::NodeHandle n;  

    ros::Subscriber sub3 = n.subscribe("array", 100, arrayCallback);

    ros::spinOnce();

    for(j = 1; j < 90; j++)
    {
        printf("%d, ", Arr[j]);
    }

    printf("\n");
    return 0;
}

void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array)
{

    int i = 0;
    // print all the remaining numbers
    for(std::vector<int>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
    {
        Arr[i] = *it;
        i++;
    }

    return;
}

特に重要なところをかいつまんでいくとまずArr[ ]という配列はグローバルに宣言していますね。
これはトピックの更新に応じてコールバック関数はこの配列にデータを突っ込んでいます。
つまりコールバック関数とメイン関数間でデータの共有をするためですね。

void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array)
{

    int i = 0;
    // print all the remaining numbers
    for(std::vector<int>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
    {
        Arr[i] = *it;
        i++;
    }

    return;
}

コールバック関数では引数を受信したInt32MultiArray型のメッセージとして
配列の先頭から末尾までのデータをArr[ ]に突っ込みます。
というかC++のconst_iteratorって言うのは便利ですね〜。

まとめ

・ROSにはたくさんの型のメッセージが使用できる
・Int32MultiArray型のメッセージではarray.data.push_back( )によってデータを格納
・コールバック関数でグローバル配列にデータを保存する

参考にしたサイト

ROSの公式Wiki　http://wiki.ros.org/
ソースコード置き場　https://gist.github.com/alexsleat/1372845