rosserial(Wi-Fi)の委譲(delegation)はポリモーフィズムの罠があった。

はじめに

個人的に、M5Stack公式のロボットである、M5BALA - ミニバランスカーのROS化を進めている。（fudekun/ros_m5bala: ROS driver for M5Bala）M5Balaは手のひらサイズでとっても「かわいいやつ」である。それでいて、価格はM5Stack Fireと合わせても1.3万円程度とオモチャとしては最高のコスパ。是非とも布教したい。

このロボットの実装に関しては、後々詳細をまとめようと思うのだが、思いっきり引っ掛かったポイントがあったのでその点について先にまとめることとする。

結論から書くと、、、

いっぱいVoteされている記事だからといって、鵜呑みにしてコピペせずによく見て使うこと。
まぁ、とりあえず急ぐ人のために。rosserial(Wi-Fi)の委譲(delegation)はWiFiHardware実装は次のようにするのが良さそうであるということを最初に書いておく。

class WiFiHardware {
  public:
  // 中略
  void write(uint8_t* data, int length) {
    client.write(data, length);    // After
    // for(int i=0; i<length; i++)    Before
    //  client.write(data[i]);
  }
  // 中略
};

rosserial(Wi-Fi)の実装を解説しながら、順を追って見ていく。

事の経緯

本日のros_m5bala進捗
xacroを作って、odomとtfを #M5stack で計算してpubするようにした。大体合ってる。
気になるのが、odomのpub周期が1Hzくらいしか出ないのはそういうものなのか、はたまた遅い原因があるのか、、、要調査。https://t.co/TfBPvxlIB7 pic.twitter.com/eB2LmLx8Qj

— T.Fukuta(fudekun) (@fudekunJP) June 2, 2019

自分の引用したツイートの通り、「odomのpub周期が1Hzくらいしか出ない」という問題にぶち当たった。
さすがに、M5Stackほどの高機能Arduino機で1Hz、1KB/sしか出ないのはおかしいと思い、いろいろ見直していくと、あやしげな点を発見した。

WiFiHardwareの送信処理が気になる。これは、データ量が増えると遅そう。。。
void write(uint8_t* data, int length) {
for(int i=0; i<length; i++)
client.write(data[i]);
}

— T.Fukuta(fudekun) (@fudekunJP) June 3, 2019

そう、コピペした部分に怪しい実装があったのだ。

rosserial(Wi-Fi)の実装

esp32やM5Stackのような、rosserial(Wi-Fi)を使ってやり取りする場合のコード記述について改めて振り返ってみる。実装方法についてググるとだいたいこの記事にたどり着くと思う。以下では、odomに何も値を突っ込んでいないが、loopなり、別コアで値を突っ込む実装を追加するぐらいでだいたいこんな感じ。

#include <ros.h>
#include <nav_msgs/Odometry.h>
#include <WiFi.h>

IPAddress server(192, 168, 0, 10);
WiFiClient client;
char ssid[] = "ssid";
char pass[] = "password";

class WiFiHardware {
  public:
  WiFiHardware() {};
  void init() {
    client.connect(server, 11411);
  }
  int read() {
    return client.read();         //will return -1 when it will works
  }
  void write(uint8_t* data, int length) {
    for(int i=0; i<length; i++)
      client.write(data[i]);
  }
  unsigned long time() {
     return millis(); // easy; did this one for you
  }
};

void setupWiFi() {
  WiFi.begin(ssid, password);
  uint8_t i = 0;
  while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
  if (i == 21) {
    while (1) delay(500);
  }
}

nav_msgs::Odometry msg;
ros::Publisher pub("odom", &msg);
ros::NodeHandle_<WiFiHardware> nh;

void setup() {
  setupWiFi();
  nh.initNode();
}

void loop() {
  pub.publish(&msg);
  nh.spinOnce();
  delay(500);
}

今回注目するのは、WiFiHardwareクラス。

NodeHandle_（rosserialのPub/Sub通信を担う）ハンドラーに、「Wi-Fi処理を記述したクラス」をテンプレートを使って委譲(delegation)することで、いろんな通信規格に対応しているわけである。
esp32のWiFiClientクラス（arduino-esp32/WiFiClient.h at master · espressif/arduino-esp32）を使って、「Wi-Fi処理を記述したクラス」実装であるWiFiHardwareクラスを書いていく。client.writeが送信、client.readが受信に関する処理である。

今回問題になったところ

そう、ループでデータを送り出すここが問題である。この実装は広く普及しているようであるが、uint8_tの8ビット表現のバイトデータ（結構よく使う表現）をぶつ切りにして送り出している。送信データが小さいうちは問題ないのであるが、odomデータのようなそこそこのペイロード（1KB程度）のデータでは送信回数が1000回を超えてくる。これは遅いはずである。

  void write(uint8_t* data, int length) {
    for(int i=0; i<length; i++)
      client.write(data[i]);
  }

WiFiClientの実装を見直そう

WiFiClient.hについて見てみると、client.write処理には4種類の実装が存在する（ポリモーフィズム）ことに気がついた。
当初の実装は1番上の実装である。2番目の実装write(const uint8_t *buf, size_t size);をcpp含めて見てみると、複数バイトのデータを指定して送信できることが分かった。（そもそも1番目の実装が、2番目の実装を1ブロック（8bit）で送信するという実装になっていた。。。）

size_t write(uint8_t data);
size_t write(const uint8_t *buf, size_t size);
size_t write_P(PGM_P buf, size_t size);
size_t write(Stream &stream);

size_t WiFiClient::write(uint8_t data)
{
    return write(&data, 1);
}

// 中略

size_t WiFiClient::write(const uint8_t *buf, size_t size) 
{
  // 中略
  res = send(socketFileDescriptor, (void*) buf, bytesRemaining, MSG_DONTWAIT); 
  // 中略
}

バルク送信を使った結果

というわけで、従来型実装の8bit毎送信モードと、バルク送信モードを使った結果を比較。

バルクの口があったので試してみたら、およそ150倍ほど爆速になった旨、報告しておきます。後から試す人のために、どこかにまとめておかなくては。

[before]
for(int i=0; i<length; i++)
client.write(data[i]);
1Hz、1KB/s

[after]
client.write(data, length);
270Hz、150KB/s pic.twitter.com/I3teOi1Oor

— T.Fukuta(fudekun) (@fudekunJP) June 3, 2019

周波数では270倍程度、送信速度では150倍程度の高速化に成功した。もはや270Hzでodomの計算は無理なので、delayを入れてpubする頻度を調整する必要さえ出てくる。画像の送信もこの様子では問題がなさそうであるという手応えを得た。これは嬉しい。

最後に

esp32は、リファレンスが無い？のが気になる。おそらく、楽せずソースを読めということでしょう。
ソースと同じくらい、英語も読めるようになりたい。