ROSノードでもスタンドアロンでも動作するArduinoデバイスを作る

# モチベーション

ROSのノードとして実デバイスを操作する手段として、ROS Serialがあります。これ自体はとても便利なのですが、何も考えずに実装するとROSに接続された時にだけ動作するデバイスが出来上がります。

普通の設計であれば特に問題はないのですが、今回は次のような仕様のデバイスを作れないか実験してみます。

- 割り込みで情報を取得する(今回はシリアル経由での割り込み)
- ROSに接続されている間は割り込みで取得された情報をPublishする
- 同じくROSに接続されている間はSubscribeによりLチカする
- 独立して動作する間は割り込みで情報を取得したタイミングでLチカする

ROS Serial Arduinoは簡単に使える分ハマりやすい部分も多いですので、同じような使い方をする方向けに何か参考になればと思い、メモを作ってみました。

# 準備

今回の実験を行うにあたり、

- ROS kinetic
- Ubuntu 16.04
- [ROS serial arduino](http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup)
- Arduino IDE

をあらかじめセットアップしておきます。
(ここでは、セットアップ手順については割愛します)


# 各ノードの実装

この実験では、Arduinoを2台使いました。
それぞれの実装は下記の通りです。

## SerialSender(Arduino Leonardo)

ひたすら文字列をシリアル通信経由で他方のArduino(SerialRecv_ROS、Megaを使いました)に送り続けます。特に難しいことはしていません。
デバッグ用にUSB Serialを使いたかったので、SoftwareSerialを利用しています。

    #include "SoftwareSerial.h"
    
    SoftwareSerial mySerial(2, 3); // RX, TX
    
    void setup() {                
        Serial.begin(9600);
        mySerial.begin(115200);
        mySerial.println("Hello from Arduino SoftwareSerial!");
    }
    
    void loop() {
        mySerial.print("MySerial!!");
        mySerial.write("\n");
        delay(100);
    }

Arduino LeonardoのD2ピンとD3ピンをそれぞれArduino MegaのD18(TX1)およびD19(RX1)に接続します。(GNDの接続も忘れずに。)
Arduino MegaのSerial1を使っているのは、Serial(USB)はROSの通信用に利用するためです。


## SerialRecv_ROS(Arduino Mega)

PCとUSBで接続し、ROS Serial Arduinoとして動作するノードです。
SerialSenderからの文字列を割り込みで受信します。
動作の状態を確認するため、ROSの接続状態にかかわらずD12に接続したLEDを点滅させています。
ROSの接続が確立されている間はその文字列をPublishします。また、D13直結のLEDを/led_switchトピックをSubscribeして点滅させます。

    #include <ros.h>
    #include <std_msgs/Empty.h>
    #include <std_msgs/String.h>
    
    #define LED_PIN 13
    #define SERIAL1_LED_PIN 12
    
    ros::NodeHandle nh;
    std_msgs::String str_msg;
    ros::Publisher serial1pub("serial1pub", &str_msg);
    
    void ledsubCb( const std_msgs::Empty& toggle_msg) {
      digitalWrite(LED_PIN, HIGH);
      delay(100);
      digitalWrite(LED_PIN, LOW);
      delay(100);
    }
    ros::Subscriber<std_msgs::Empty> ledsub("led_switch", &ledsubCb );
    
    
    bool serial1LedStat = false;
    void serial1Led(){
      if(serial1LedStat){
        digitalWrite(SERIAL1_LED_PIN, HIGH);
        serial1LedStat = false;
      }else{
        digitalWrite(SERIAL1_LED_PIN, LOW);
        serial1LedStat = true;
      }
    }
    
    String str = "";
    bool isRosConnected = false;
    void serialEvent1(){
      if (Serial1.available()>0){
        str = Serial1.readStringUntil("\n");
        serial1Led();
        if(isRosConnected){
          int len = str.length();
          char charArray[len];
          str.toCharArray(charArray, len);
          str_msg.data=charArray;
          serial1pub.publish(&str_msg);
          }
        }
    }
    
    void setup()  {
      Serial1.begin(115200);
      Serial1.setTimeout(100);
      pinMode(LED_PIN, OUTPUT);
      pinMode(SERIAL1_LED_PIN, OUTPUT);
    }
    
    
    void loop() {
      if(nh.connected()){
        isRosConnected = true;
        nh.spinOnce();
      }
      else{
        isRosConnected = false;
        nh.initNode();
        nh.subscribe(ledsub);
        nh.advertise(serial1pub);
        nh.spinOnce();
      }
      delay(1);
    }



# 実験の結果

上記に示した実装にて、ROSと接続されている間はROS Serial Arduinoのノードとして、接続が切れている間は独立したノードとして動作することが確認できました。
確認ポイントは以下の通りです。

- PC側で接続(rosrun rosserial_python serial_node.py /dev/ttyACMx)しなくても、Serial1の受信LEDが点滅していること
- PCと接続した後、Serial1の受信結果がPCからSubscribeできる(rostopic echo /serial1pub)こと
- PCからのPublish(rostopic pub led_switch std_msgs/Empty --once)結果により、D13のLEDが点滅すること
- PCからの接続を切断しても、Serial1の受信が継続できる(Serial1受信LEDが点滅し続ける)こと

Arduino Mega側の実装に関していくつか注意すべき点が見えたので、記載します。

## serialEvent1

Serial1(D18,19)に対してイベントが発生すると呼び出される関数です。
ここでSerial1に対する受信処理を行います。
Serial1.readStringUntil("\n”)を使うことで、終端文字を含む文字列をStringオブジェクトとして取り込むことができます。
serialEvent1は割り込み関数ですが、実際にはloop()の末尾で動作するため、setup()内でROSの接続を待ってしまうと接続が確立するまでserialEvent1を実行することができなくなってしまいます。
また、loop()内で時間のかかる処理を実行してしまうとSerial1の取りこぼしが発生してしまいますので注意が必要です。


## NodeHandle

ROSノードとしての動作を管理するオブジェクトです。
多くのサンプルでは、setup()の中でNodeHandleの初期化処理(nh.initNode())を実行していますが、これでは上述のserialEvent1をうまく動作させることができません。
また、一度ROSの接続が切れてしまうとArduino側をリセットしないと再度接続することができませんので、今回はloop()の中で接続判定・再接続処理を行うようにしました。

**spinOnce**
Arduino側にPublishされてきた際のコールバックは、nh.spinOnce()の部分でポーリングを行ってまとめて処理されているようです。そのため、定期的にspinOnceを呼び出さないとコールバックが正しく実行できなくなってしまいます。

**initNode**
ノンブロッキングな関数のようです。接続を試行したら、ROSのマスターとの情報同期を図るため、すぐにspinOnceを呼び出すのがよいでしょう。


# 終わりに

いかがでしたでしょうか。
今回は各ロジックの挙動を中心に実験していきましたが、機会があれば性能を中心に検証していきたいと思います。