モチベーション
ROSのノードとして実デバイスを操作する手段として、ROS Serialがあります。これ自体はとても便利なのですが、何も考えずに実装するとROSに接続された時にだけ動作するデバイスが出来上がります。
普通の設計であれば特に問題はないのですが、今回は次のような仕様のデバイスを作れないか実験してみます。
- 割り込みで情報を取得する(今回はシリアル経由での割り込み)
- ROSに接続されている間は割り込みで取得された情報をPublishする
- 同じくROSに接続されている間はSubscribeによりLチカする
- 独立して動作する間は割り込みで情報を取得したタイミングでLチカする
ROS Serial Arduinoは簡単に使える分ハマりやすい部分も多いですので、同じような使い方をする方向けに何か参考になればと思い、メモを作ってみました。
準備
今回の実験を行うにあたり、
- ROS kinetic
- Ubuntu 16.04
- ROS serial arduino
- 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を呼び出すのがよいでしょう。
終わりに
いかがでしたでしょうか。
今回は各ロジックの挙動を中心に実験していきましたが、機会があれば性能を中心に検証していきたいと思います。