ROSの覚え書き（ArduinoのSubscriber編）

今回は、子供の自由研究で製作したArduino UNOを用いたロボットカーを、
ROS経由で制御してみる。

hirotaka-hachiya.hatenablog.com

「roserial」というROSのシリアル通信プロトコルを用いると、Arduinoなどシリアル通信で接続可能なハードウェアを、ROSノードにし、ROSのメッセージであるtopicをpublish（送信）およびsubscribe（受信）できるようすることができる。ArduinoがROSノードになると、Arduinoに接続しているセンサを、他のロボットやPCから読み取ったりすることができるようになる。

rosserialを利用するためには、roscoreを実行し、Arduino IDEでプログラム開発するPCに、rosserialとrosserial-arduinoパッケージをインストールする必要がある。
下記のページに従い、これらのパッケージをインストールした。
http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup

> sudo apt-get install ros-indigo-rosserial
> sudo apt-get install ros-indigo-rosserial-arduino

Arduino IDEの「ファイル」->「環境変数」->「スケッチブックの保存場所」
にて設定されているディレクトリを確認する。私の場合は、「/home/hachiya/Works/Arduino」に設定されている。
該ディレクトリに移動し、下記のようにArduino IDEからROSのライブラリ（ros_lib）を読み込めるように設定する。

> cd /home/hachiya/Works/libraries
> rm -rf ros_lib
> rosrun rosserial_arduino make_libraries.py .

これで、Arduino IDEにてROSライブラリを用いる準備ができた。

次に、ROSのサンプルプログラムを読み込む。Arduino IDEの「ファイル」->「スケッチの例」->「ros_lib」から「Blink」を選択する。これは、ArduinoのD13ピンに接続されたLEDを点灯または消灯するrosserialのプログラムコードの例である。配線方法は、前回紹介した通りである。hirotaka-hachiya.hatenablog.com

/* 
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */

#include <ros.h>
#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

void messageCb( const std_msgs::Empty& toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
}

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

void setup()
{ 
  pinMode(13, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{  
  nh.spinOnce();
  delay(1);
}

プログラムコードの解説は、下記のページに記載されている。
http://wiki.ros.org/ja/rosserial_arduino/Tutorials/Blink

以下要点だけを説明する。

下記のようにrosserailを利用するプログラムでは、必ずros.hとROSメッセージのヘッダファイル（std_msgs/String.hや、std_msgs/Empty.h）
をインクルードする。

#include <ros.h>
#include <std_msgs/Empty.h>

このROSメッセージのヘッダーファイルは、下記のようにros_lib/std_msgsディレクトリにある。

> ls std_msgs
Bool.h               Header.h               String.h
Byte.h               Int16.h                Time.h
ByteMultiArray.h     Int16MultiArray.h      UInt16.h
Char.h               Int32.h                UInt16MultiArray.h
ColorRGBA.h          Int32MultiArray.h      UInt32.h
Duration.h           Int64.h                UInt32MultiArray.h
Empty.h              Int64MultiArray.h      UInt64.h
Float32.h            Int8.h                 UInt64MultiArray.h
Float32MultiArray.h  Int8MultiArray.h       UInt8.h
Float64.h            MultiArrayDimension.h  UInt8MultiArray.h
Float64MultiArray.h  MultiArrayLayout.h

次に、ROSメッセージをやりとりするpublisherおよびsubscriberのROSノードのインスタンス「nh」を作る。

ros::NodeHandle nh;

次に、「toggle_led」というtopicをやりとりし、メッセージの種類として「std_msgs::Empty」を扱い、callback関数として「messageCb」を持つ
Subscriberのインスタンス「sub」を作る。

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );」

callback関数「messageCb」は次のように定義されている。callback関数では、メッセージインスタンスへの参照を引数にとる。
ここでは、std_msgs::Empty型の「toggle_msg」というメッセージインスタンスへの参照を引数にとっているが、
メッセージがEmpty「空」のため、callback関数内では、該メッセージインスタンスを参照していない。
callback関数内では、LEDの点灯または消灯をしている。

void messageCb( const std_msgs::Empty& toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
}

そして、setup関数では、ROSノードのインスタンス「nh」を初期化し、Subscriber「sub」を開始する。

nh.initNode();
nh.subscribe(sub);

最後に、loop関数では、publisherからのメッセージを受け取りコールバック関数を呼び出すspinOnceを実行する。

nh.spinOnce();

該プログラムコードを、Arduino UNOに書き込んだ後、USBケーブルを繋いだまま、
３つのシェルでそれぞれ次のコマンドを実行する。

>roscore

>rosrun rosserial_python serial_node.py /dev/ttyACM0

>rostopic pub toggle_led std_msgs/Empty -r 2

最後のコマンドは、publisherとして、トピック「toggle_led」に、「std_msgs/Empty」のメッセージを、1秒間に2回送るというものである。
これにより、LEDは0.5秒間隔で点灯と消灯を繰り返す。

最後にノード間のやりとりを可視化してみる。

> rosrun rqt_graph rqt_graph
> rostopic echo /toggle_led

---

---

---