以下の内容はhttps://fluorite36.hatenablog.com/entry/2021/01/19/050437より取得しました。

rosserialでIO操作する

はじめに

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 ROSのノード間通信をシリアル通信越しに行なうためのプロトコル（決めごと）。rosserial - ROS Wiki
rosserial_client rosserialのプロトコルで通信するためのマイコン（クライアント）側のライブラリ。rosserial_client - ROS Wiki
rosserial_arduino rosserial_clientをArduinoで使えるようにするROSパッケージ。その中のArduinoライブラリがros_lib。rosserial_arduino - ROS Wiki
rosserial_python PC（ホスト）側のROSパッケージ。Python実装版。rosserial_python - ROS Wiki
serial_node rosserial_pythonのノード。このノード越しにマイコンと通信する。他のノードとノード間通信をして、マイコンの面倒もみる。pyserialでマイコンとシリアル通信をして、もらったメッセージをマイコンに送ったり、マイコンから受け取ったメッセージを別のノードに送ったりする。これにより、ノード間通信をする感覚で、マイコンと通信できる。
xx.ino いつものArduinoのコード。rosserialプロトコルを使うためのros_libライブラリから必要なものをインクルードして、どういったROSメッセージをやりとりするのかを追記する。

名前がわかりづらいので、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

２つのノードに分けて使っていました。 Arduino側ノードに直接〇〇信号メッセージ、xx信号メッセージを送るのではなく、お膳立てノードを一段挟んでいます。 Arduino側のコードは一発書いたらずっとそのままで、デジタルピンの使い道を変えたかったらお膳立てノード側の処理を変更するだけで対応できます。 ピンの使い方を変えようと思ったら、Arduino側のコードを変えて、通信方法を考えなければいけないしいちいちArduinoに書き込んだりするのが手間だと思ってこのような構成にしました。このArduino側のコードを紹介しようと思います。

（間に何も挟まずに直接やりとりするのが普通だと思います。 お膳立てノードでやっている、複数ピンの状態を表す2進数への変換をするといったぐらいの処理ならそのままマイコンでやってしまえます。 何なら反射みたいに、PC介さずにマイコン内でピン入力から条件分岐してそのまま出力を変化させるぐらいやってしまった方が高速に反応できるしROS側との通信も少なくてよいとか色々あると思います。今回の使用用途的には間に合っていたので、この構成になっています。）

以下Arduinoのコードと、その説明です。

/* rosserial

---
$ roscore
---
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
---
$ rostopic list
---
$ 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

$ rostopic echo /read_pin

$ rostopic pub -1 /io_sendLog_flag std_msgs/Bool "data: true"
$ rostopic pub -1 /io_sendLog_flag std_msgs/Bool "data: false"

---

Arduino UNO

 0    HardwareSerial RX
 1    HardwareSerial TX
 2    digitalWrite
 3    digitalWrite
 4    digitalWrite
 5    digitalWrite
 6    digitalWrite
 7    digitalWrite
 8    digitalWrite
 9    digitalWrite
10    digitalWrite
11    digitalWrite
12    digitalWrite
13    digitalWrite
14 A0 digitalWrite
15 A1 digitalRead
16 A2 digitalRead
17 A3 digitalRead
18 A4 digitalRead
19 A5 digitalRead

pin mapping
-> #define, output_pin[], input_pin[]

*/

// -------------------------------------------------
#include <ros.h>
#include <std_msgs/UInt8.h>   // publish    5 pin input
#include <std_msgs/UInt16.h>  // subscribe 13 pin output
#include <std_msgs/Bool.h>    // subscribe sendLog flag

ros::NodeHandle nh;
std_msgs::UInt16 msg_writePin;
std_msgs::UInt8  msg_readPin;
std_msgs::Bool   msg_sendLogFlag;

// settings -----------------------------------------
#define USE_INPUTPULLUP
//   ifdef use pinMode( pin, INPUT_PULLUP);

#define BOARD_PINS_NUM        20  // 0~19
#define OUTPUT_PIN_NUM        13
#define OUTPUT_PIN_USE_BITMASK (0b0001111111111111) // use pin:1
#define INPUT_PIN_NUM          5
#define INPUT_PIN_USE_BITMASK  (0b00011111)

// pin mapping
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 ] = // +\0
{
  "RT000000000000000000"
  // rx, tx,  0, 0, 0,...
};
char charBuf[100];

// prototype declaration
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 // [ms]  1000[ms] / 50[Hz]
#define INFO_SENDLOG_INTERVAL 20 // [ms]  1000[ms] / 50[Hz]
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()
{
  // pin init
  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();

  // pin read ----------------------------------------------
  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();

  // send log ----------------------------------------------
  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)
{
  // update writepin state
  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 );
    }
  }
}
//    ex. data = 6 = 0b0110  ↓
//      i = 0, 6 >> 0 = 0b011|0|, data & 1 = 0
//      i = 1, 6 >> 1 = 0b001|1|, data & 1 = 1
//      i = 2, 6 >> 2 = 0b000|1|, data & 1 = 1
//      i = 3, 6 >> 3 = 0b000|0|, data & 1 = 0
//      ...                   ^

void messageCbSendLog( const std_msgs::Bool &msg)
{
  msg_sendLogFlag = msg;
}

// write and memory
void digitalOut(int pin, int state)
{
  digitalWrite(pin, state);
  
  if(state == HIGH)
  {
    pin_state_log[ pin ] = '1';
  }else{
    pin_state_log[ pin ] = '0';
  }
}

// read and memory
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"
  // rx, tx,  0, 0, 0,...

といったような感じでピンの入出力状態のデバッグ出力をします。

ifdef で、入力をINPUT_PULLUPにするかどうか選択できるようにしています。

#define USE_INPUTPULLUP

シリアル通信用の2ピンを飛ばして、D2~D13,A0を出力ピン、A1~A5を入力ピンとしています。 配列にいれて置き換えているので、順番等自由に変えられます。

入力ピンの状態をpublishする周期、ピンの入出力状態をデバッグ出力する周期をmsで設定します。

#define PUB_READPIN_INTERVAL  20 // 1000 / 50
#define INFO_SENDLOG_INTERVAL 20 // 1000 / 50

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


以上の内容はhttps://fluorite36.hatenablog.com/entry/2021/01/19/050437より取得しました。
このページはhttp://font.textar.tv/のウェブフォントを使用してます
不具合報告/要望等はこちらへお願いします。
モバイルやる夫Viewer Ver0.14