rosserial_arduinoで複数のpublisherをプログラマブル（動的）に作成する方法

背景

rosserial_arduinoとは、ROSというロボットに関する機能を使いやすく提供してくれるプログラムで、Arduinoというマイコン（プログラムを書き込める開発板）を利用するためのライブラリです。

publisherとはROSで情報をやりとりするための概念の１つです。
その名の通り、値を発信する役割を持ちます。

rosserial_arduinoのpublisherのサンプルプログラムでは、publisherをsetupの前に変数として定義して利用しています。
しかし、「sensor0」「sensor1」「sensor2」など、forループでpubliserを初期化できたら嬉しい場面がありますが、そのようなサンプルプログラムは無さそうでした。

自分が知っているc++やArduino言語の機能を駆使してforを利用したpublisherの初期化はできたのですが、詰まる部分があったので、備忘録を兼ねて方法を共有します。

使ったもの

ROS

Ubuntu18.04 + Melodicを使いました。

melodic/Installation/Ubuntu - ROS Wiki

rosserial_arduino

aptコマンドでインストールするか、wsのsrcにソースコードを置いてcatkin_buildすると使えるようになります。

rosserial_arduino - ROS Wiki
自作した台車をrosserial（ROS）で制御する方法

PlatformIO

rosserial_arduinoをライブラリとして利用したいので、Arduino IDEではなくこちらを利用しました。

プロジェクトのlib_depsに記述すれば、rosserial_arduinoを使えるようになります。

platformio.ini

lib_deps = rosserial_arduino

PlatformIO
PlatformIOを使うと、Arduinoのプロジェクトのライブラリの管理が楽そうだった話

Pro Micro

Arduino Leonardoの互換機版として利用できる製品です。

Pro Micro 5V/16MHz - スイッチサイエンス
Pro Micro | Aliexpress

うまくいった書き方

うまくいったコードがこちらです。
publisherをポインタの配列として定義し、setupでpubNamesに名前を保持しつつ、publisherをnewしているのが鍵です。

src/main.cpp

hinclude <Arduino.h>

#ifdef USBCON // for leonardo
#define USE_USBCON
#endif

#include <ros.h>
#include <sensor_msgs/Range.h>

unsigned const char SENSOR_PINS[] = {
  A0,
  A1,
};
const int SENSOR_NUM = sizeof(SENSOR_PINS) / sizeof(SENSOR_PINS[0]);

ros::NodeHandle nh;
sensor_msgs::Range range[SENSOR_NUM];
ros::Publisher *pubs[SENSOR_NUM];
String pubNames[SENSOR_NUM];

void setup() {
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  for (int i = 0; i < SENSOR_NUM; ++i) {
    pubNames[i] = "sensor" + String(i);
    pubs[i] = new ros::Publisher(pubNames[i].c_str(), &range[i]);
    nh.advertise(*pubs[i]);
  }
}

void loop() {
  for (int i = 0; i < SENSOR_NUM; ++i) {
    range[i].range = analogRead(SENSOR_PINS[i]);
    pubs[i]->publish(&range[i]);
  }
  nh.spinOnce();
  delay(100);
}

pubNamesがなぜ必要なのか、詰まったときの書き方を示しながら説明します。

topicを読み込めずに詰まった書き方と、その時のエラー

pubNamesを使わずStringのc_strを使ってpublisherをnewすると、ビルドは通ります。

ビルドは通るが、実行時に不具合が発生する記述

void setup() {
  for (int i = 0; i < SENSOR_NUM; ++i) {
    pubs[i] = new ros::Publisher(("sensor" + String(i)).c_str(), &range[i]);
  }
}

しかし、上記のpubNamesを使わないコードだと、rosserial_pythonを実行すると下記のエラーが出てtopicが読み込めない不具合が発生します。

[INFO] [1551878102.653449]: Requesting topics...
[ERROR] [1551878102.775391]: Creation of publisher failed: topic name is not a non-empty string
[ERROR] [1551878102.777922]: Creation of publisher failed: topic name is not a non-empty string
[ERROR] [1551878102.818568]: Tried to publish before configured, topic id 125

ArduinoのStringのc_strは、Stringのバッファを直接読み込む仕組みになっているようです。
そのため、c_strをPubliser new時の引数に直接渡すと、c_strを実行したStringの参照元が無くなり、c_strが空文字になってしまうようです。
（メモリの状態によっては、意図しない文字になることもあります。）

c_strの参照元が無くなってしまう対策として、publisherの名前を意味するStringをpubNamesという名前のグローバル変数で管理しました。
これによって文字列が保持し続けられるようになり、エラー無くrosserial_pythonでtopicが読み込めるようになりました。

うまく動く記述

String pubNames[SENSOR_NUM];

void setup() {
  for (int i = 0; i < SENSOR_NUM; ++i) {
    pubNames[i] = "sensor" + String(i);
    pubs[i] = new ros::Publisher(pubNames[i].c_str(), &range[i]);
  }
}

まとめ

ビルドできるのに動かない症状に戸惑いましたが、プログラマブルにpublisherを作成できました。
確認はしていませんが、多分同じような方法でSubscriberもプログラマブルに作れると思います。

何かの参考になれば嬉しいです。

参考

c_str() | Arduino Reference
rosserial_arduinoTutorialsHello World - ROS Wiki