はじめに
ROSのノード間通信をシリアル通信越しに行なうためのプロトコルが、rosserialです。このArduino向けの実装がrosserial_arduinoです。(初めて名前見たときは、ROSで直接シリアル通信をするパッケージだと勝手に思い込んでいました。)
これを使ってROSからArduino UNOのIOを動かして、ロボに指令を送ったり状態を読んだりするプログラムを書きました。ふと思い立ったので公開しようと思います。
github.com
こんな感じのことができるようになります。
動作を確認した環境は、Ubuntu16.04 ROS 1 Kinetic Kameです。
今更感がすごいのですが、ふと前に書いてたことを思い出したのと、その時にサンプルが少なくて困ったのを思い出して公開しようと思いました。
新しい環境でも動くか試して記事を更新したい。
rosserial
環境構築については最新の情報をこちらから
rosserial_arduino/Tutorials - ROS Wiki
rosserial_なんたら という名前のものがたくさんあったりしてややこしくて混乱して調べたメモです。
間違っていたらご指摘お願いいたします。
名前がわかりづらいので、rosserial_host_hogehoge、rosserial_client_hogehogeみたいな名前にならないかしら←
rosserialでIO操作するコード
IO操作で動かせるロボがあって、ROSで動かせるようにしたいなと思って書いたものです。
- Arduino Uno 入出力設定
- シリアル通信 ROSとの通信で使用 ( D0,D1 Rx, Tx )
- デジタル出力 13pin ( D2~D13, A0 )
- デジタル入力 5pin ( A1~A5 )
ノード構成
- お膳立てノード
- 「〇〇信号を出して」「××信号を3にして」等といった指示をsubscribe
- Arduinoの出力ピンの上げ下げを表す2進数へ変換してwrite_pinメッセージとしてpublish
- Arduinoノード .ino(serial_node)
- デジタル出力するピンの情報であるwrite_pinメッセージをsubscribe
- デジタル入力するピンの情報であるread_pinメッセージをpublish
- 各ピンの入出力状態のデバッグ出力をするフラグであるio_sendLog_flagメッセージをsubscribe
2つのノードに分けて使っていました。
Arduino側ノードに直接〇〇信号メッセージ、xx信号メッセージを送るのではなく、お膳立てノードを一段挟んでいます。
Arduino側のコードは一発書いたらずっとそのままで、デジタルピンの使い道を変えたかったらお膳立てノード側の処理を変更するだけで対応できます。
ピンの使い方を変えようと思ったら、Arduino側のコードを変えて、通信方法を考えなければいけないしいちいちArduinoに書き込んだりするのが手間だと思ってこのような構成にしました。このArduino側のコードを紹介しようと思います。
(間に何も挟まずに直接やりとりするのが普通だと思います。
お膳立てノードでやっている、複数ピンの状態を表す2進数への変換をするといったぐらいの処理ならそのままマイコンでやってしまえます。
何なら反射みたいに、PC介さずにマイコン内でピン入力から条件分岐してそのまま出力を変化させるぐらいやってしまった方が高速に反応できるしROS側との通信も少なくてよいとか色々あると思います。今回の使用用途的には間に合っていたので、この構成になっています。)
以下Arduinoのコードと、その説明です。
#include <ros.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Bool.h>
ros::NodeHandle nh;
std_msgs::UInt16 msg_writePin;
std_msgs::UInt8 msg_readPin;
std_msgs::Bool msg_sendLogFlag;
#define USE_INPUTPULLUP
#define BOARD_PINS_NUM 20
#define OUTPUT_PIN_NUM 13
#define OUTPUT_PIN_USE_BITMASK (0b0001111111111111)
#define INPUT_PIN_NUM 5
#define INPUT_PIN_USE_BITMASK (0b00011111)
const int output_pin[ OUTPUT_PIN_NUM ] =
{
2,
3,
4,
5,
6,
7,
8,
9,
10,
11,
12,
13,
14
};
const int input_pin[ INPUT_PIN_NUM ] =
{
15,
16,
17,
18,
19
};
char pin_state_log[ BOARD_PINS_NUM +1 ] =
{
"RT000000000000000000"
};
char charBuf[100];
void digitalOut(int pin, int state);
int digitalIn(int pin);
void messageCbWritePin( const std_msgs::UInt16 &msg);
void messageCbSendLog( const std_msgs::Bool &msg);
#define PUB_READPIN_INTERVAL 20
#define INFO_SENDLOG_INTERVAL 20
unsigned long timeMs = 0;
unsigned long timeMsBuf[2] = {0};
ros::Subscriber<std_msgs::UInt16> sub_writePin("write_pin", &messageCbWritePin);
ros::Publisher pub_readPin ("read_pin", &msg_readPin);
ros::Subscriber<std_msgs::Bool> sub_sendLog ("io_sendLog_flag", &messageCbSendLog);
void setup()
{
for(int i = 0; i< OUTPUT_PIN_NUM; i++)
{
pinMode(output_pin[ i ], OUTPUT);
digitalOut(output_pin[ i ], LOW);
}
for(int i = 0; i< INPUT_PIN_NUM; i++)
{
#ifdef USE_INPUTPULLUP
pinMode(input_pin[ i ], INPUT_PULLUP);
#else
pinMode(input_pin[ i ], INPUT);
#endif
digitalIn(input_pin[ i ]);
}
msg_writePin.data = 0;
msg_readPin.data = 0;
msg_sendLogFlag.data = false;
nh.initNode();
nh.advertise(pub_readPin);
nh.subscribe(sub_writePin);
nh.subscribe(sub_sendLog);
}
void loop()
{
timeMs = millis();
if( timeMs - timeMsBuf[0] >= PUB_READPIN_INTERVAL )
{
msg_readPin.data = readPinState() & INPUT_PIN_USE_BITMASK;
pub_readPin.publish(&msg_readPin);
timeMsBuf[0] = timeMs;
}
nh.spinOnce();
if( msg_sendLogFlag.data )
{
if( timeMs - timeMsBuf[1] >= INFO_SENDLOG_INTERVAL )
{
nh.loginfo( pin_state_log );
timeMsBuf[1] = timeMs;
}
}
}
void messageCbWritePin( const std_msgs::UInt16 &msg)
{
msg_writePin = msg;
msg_writePin.data = msg.data & OUTPUT_PIN_USE_BITMASK;
int state = 0;
for(int i = 0; i< OUTPUT_PIN_NUM; i++)
{
state = (msg_writePin.data >> i) & 1;
if(state == 1)
{
digitalOut(output_pin[ i ], HIGH );
}else{
digitalOut(output_pin[ i ], LOW );
}
}
}
void messageCbSendLog( const std_msgs::Bool &msg)
{
msg_sendLogFlag = msg;
}
void digitalOut(int pin, int state)
{
digitalWrite(pin, state);
if(state == HIGH)
{
pin_state_log[ pin ] = '1';
}else{
pin_state_log[ pin ] = '0';
}
}
int digitalIn(int pin)
{
int state = digitalRead(pin);
#ifdef USE_INPUTPULLUP
if( state == LOW )
{
pin_state_log[ pin ] = '1';
}else{
pin_state_log[ pin ] = '0';
}
return 1 - state;
#else
if( state == HIGH )
{
pin_state_log[ pin ] = '1';
}else{
pin_state_log[ pin ] = '0';
}
return state;
#endif
}
uint8_t readPinState()
{
uint8_t readBuf = 0;
for(int i = 0; i < INPUT_PIN_NUM; i++)
{
readBuf += digitalIn(input_pin[ i ]) << i;
}
return readBuf;
}
次のメッセージをやり取りします。
std_msgs::UInt16 msg_writePin;
std_msgs::UInt8 msg_readPin;
std_msgs::Bool msg_sendLogFlag;
log出力レベル INFO で
"RT000000000000000000"
といったような感じでピンの入出力状態のデバッグ出力をします。
ifdef で、入力をINPUT_PULLUPにするかどうか選択できるようにしています。
#define USE_INPUTPULLUP
シリアル通信用の2ピンを飛ばして、D2~D13,A0を出力ピン、A1~A5を入力ピンとしています。
配列にいれて置き換えているので、順番等自由に変えられます。
入力ピンの状態をpublishする周期、ピンの入出力状態をデバッグ出力する周期をmsで設定します。
#define PUB_READPIN_INTERVAL 20
#define INFO_SENDLOG_INTERVAL 20
millis()とタイムスタンプを比較して、ここで指定した周期ごとに通信を出力を行ないます。
やりとりするROSメッセージの設定です。
ros::Subscriber<std_msgs::UInt16> sub_writePin("write_pin", &messageCbWritePin);
ros::Publisher pub_readPin ("read_pin", &msg_readPin);
ros::Subscriber<std_msgs::Bool> sub_sendLog ("io_sendLog_flag", &messageCbSendLog);
ここで指定したコールバック関数が各メッセージ受信、送信時に呼ばれて
デジタル入出力等を行ないます。
ピンの入出力はdigitalRead,digitalWriteを直接呼ばずに、digitalIn,digitalOutという関数経由で呼び、
このときに、デバッグ用のピン入出力状態の変数を一緒に更新しています。
INPUT_PULLUPとの読み替えもここでやっています。
動かしてみる
上のinoファイルをArduinoに書き込みます。
書き込んだら、ターミナルから試しにコマンドでメッセージを送って、Arduinoと通信してみます。
以下がその例です。
serial_nodeを動かして、マイコンと通信する。
$ roscore
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
Topic通信一覧を見る
$ rostopic list
ピンの入出力状態のデバッグ出力フラグ io_sendLog_flagを送る(publishする)例
trueでデバッグ出力する。falseでデバッグ出力しない。
$ rostopic pub -1 /io_sendLog_flag std_msgs/Bool "data: true"
$ rostopic pub -1 /io_sendLog_flag std_msgs/Bool "data: false"
write_pinメッセージを送る(publishする)例
13ピン分で、13bit 0b0~0b1111111111111(0~8191)
$ rostopic pub -1 /write_pin std_msgs/UInt16 0
$ rostopic pub -1 /write_pin std_msgs/UInt16 1
...
$ rostopic pub -1 /write_pin std_msgs/UInt16 8191
read_pinメッセージを受ける(subscribeする)例
$ rostopic echo /read_pin