ROS2で自作ロボットの開発ツールを整える

概要

この記事は「自作ロボットにROS2を使ってみたら快適だった」というお話です。
細かいノウハウ情報がありますが、そもそも私のロボット専用のコードなので、マネできるエッセンスがあったらラッキーぐらいで読んで下さい。

自作ロボットにROS2を活用する

私が作った自作ロボット（マウス）ではROS2 dashing環境で動作する次の機能を利用しています。

• ros2arduino
• Micro-XRCE-Agent
• tf2
• rviz2
• rqt_plot
• visualization_marker
• 他にも色々

ロボットが競技において性能を発揮するために活用しているわけではなく、開発・デバッグを気持ちよくやりたいというモチベーションでROS2を利用してみました。

マイクロマウス

日本では1980年から継続して開催されている「マイクロマウス」というロボット競技会があります。ご存知でしょうか？最も歴史あるロボット競技と公式に書かれていますが、古くから多くのエンジニアもチャレンジしているロボット競技です。
競技では、大会毎に設定される課題（迷路）について、開始位置からゴールまでいかに速く、効率よく、知的に走行するかが評価されます。
私も2年ほど前から取り組み初めて細々とチャレンジしています。
そんな古くからある競技に、ROS2のような新しい技術を取り入れてみるのも面白いかなと思いました。

ロボットの仕様

回転数のわかる車輪、壁までの距離がわかる光センサーx4、ToFセンサーx3、6軸IMUが搭載されています。
CPUはSTM32F405 168MHzで、1msec毎に機体制御を行っています。

このマウスをROS2ノードにするためにESP8266を増設しました。小さくまとめたかったので最低限の電源と通信ができるだけの基板をエイヤっと作って実装してあります。

ESP8266がマウスからセンサー情報などを吸い上げて、無線通信でROS2の世界との橋渡しを行います。

ros2arduino

ROBOTISが公開している素晴らしいパッケージです。これを使ってパソコン上のROS2とつなげるだけであなたも私もROS2対応！スゴイ！
利用するには、ros2arduinoとMicro-XRCE-DDS AgentをReadmeに従ってセットアップします。

Micro-XRCE-DDS Agent/Client

Micro-XRCE-DDSの仕組みは、公式ドキュメントの図を引用すると次のような構成で動作します。

Micro-XRCE-DDSを開発しているeProsimaのドキュメントがちゃんと整備されているので読み解くと詳細が理解できます。
ros2arduinoはこの図でいうところの"Micro XRCE-DDS Client"を実装したものになっており、Agentへ接続することでROS2の世界と通信することができます。

落とし穴

公式のマニュアル通りにインストールするとなんとなく動きそうなところまでいくのですが、私の場合は次のように通信を開始した直後に異常終了する症状が発生してしまいました。

$ MicroXRCEAgent udp -p 2018
Enter 'q' for exit
[1576704171.510454] info     | UDPServerLinux.cpp | init                     | running...             | port: 2018
[1576704179.704237] info     | Root.cpp           | create_client            | create                 | client_key: 0xAABBCCDD, session_id: 0x81
[1576704179.704421] info     | UDPServerBase.cpp  | on_create_client         | session established    | client_key: 0xAABBCCDD, address: 192.168.5.190:47138
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
中止 (コアダンプ)

調べていくと、どうやらROS2のsetup.bashを読み込んでから実行すると異常終了します。ROS2 dashing環境に入っているライブラリのバージョン違いが悪さをしている？

$ ldd /usr/local/bin/MicroXRCEAgent
	linux-vdso.so.1 (0x00007ffd853eb000)
	libmicroxrcedds_agent.so.1 => /usr/local/lib/libmicroxrcedds_agent.so.1 (0x00007fb6447ba000)
	libstdc++.so.6 => /usr/lib/x86_64-linux-gnu/libstdc++.so.6 (0x00007fb644431000)
	libgcc_s.so.1 => /lib/x86_64-linux-gnu/libgcc_s.so.1 (0x00007fb644219000)
	libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x00007fb643e28000)
★  libfastrtps.so.1 => /opt/ros/dashing/lib/libfastrtps.so.1 (0x00007fb643692000)
	libpthread.so.0 => /lib/x86_64-linux-gnu/libpthread.so.0 (0x00007fb643473000)
★  libfastcdr.so.1 => /opt/ros/dashing/lib/libfastcdr.so.1 (0x00007fb643261000)
	librt.so.1 => /lib/x86_64-linux-gnu/librt.so.1 (0x00007fb643059000)
	/lib64/ld-linux-x86-64.so.2 (0x00007fb644d44000)
	libm.so.6 => /lib/x86_64-linux-gnu/libm.so.6 (0x00007fb642cbb000)
	libdl.so.2 => /lib/x86_64-linux-gnu/libdl.so.2 (0x00007fb642ab7000)
	libtinyxml2.so.6 => /usr/lib/x86_64-linux-gnu/libtinyxml2.so.6 (0x00007fb6428a3000)
	libssl.so.1.1 => /usr/lib/x86_64-linux-gnu/libssl.so.1.1 (0x00007fb642616000)
	libcrypto.so.1.1 => /usr/lib/x86_64-linux-gnu/libcrypto.so.1.1 (0x00007fb64214b000)

libfastrtps.soとlibfastcdr.soだけdashing側のライブラリを参照していて何やら怪しいですね。（何かインストール手順を間違えてる可能性は有）
こんな時に便利なのがLD_PRELOADです。次のように書くと使用するライブラリを無理矢理指定できます。

$ LD_PRELOAD="/usr/local/lib/libfastrtps.so.1 /usr/local/lib/libfastcdr.so.1" MicroXRCEAgent udp -p 2018

このようにするとMicroXRCEAgentがクラッシュせず動作するようになりました。

ロボットに活用してみる

ros2arduinoの基本的な使い方はexampleを読むとわかるので割愛します。
マウスの場合、次のような情報をROS2から参照できるとデバッグに有効でした。

• 現在位置（オドメトリ）
• 車輪速度
• 進行方向
• ToFの検出距離
• バッテリー電圧
• 迷路の壁検出状態

リポジトリを眺めるとros2arduinoに実装されているメッセージが読み取れますが、TurtleBot3のような移動台車ロボットで使うようなものは揃っている感じです。単発の適当な値（バッテリー電圧など）はstd_msgsでも投げられますしよくまとまっていると感じます。

トピック通信する

マイコン上にPub/Subができるノードを1つ立ちあげられるので、サンプルに沿って実装すると簡単にROS2と通信できるようになります。例えば次のようにPublisherを定義しています。

// Publisherクラス
class MousePub : public ros2::Node
{
public:
  //コンストラクタ
  MousePub()
  : Node("mouse_node")
  {
    // オドメトリ
    ros2::Publisher<nav_msgs::Odometry>* odom_pub_ = this->createPublisher<nav_msgs::Odometry>("odom");
    this->createWallFreq(PUBLISH_FREQUENCY_ODOM, (ros2::CallbackFunc)publishOdom, nullptr, odom_pub_);
    // バッテリー
    ros2::Publisher<std_msgs::Float64>* batt_pub_ = this->createPublisher<std_msgs::Float64>("batt");
    this->createWallFreq(PUBLISH_FREQUENCY_BATT, (ros2::CallbackFunc)publishBatt, nullptr, batt_pub_);
    // ToF 正面
    ros2::Publisher<sensor_msgs::Range>* tof_front_pub_ = this->createPublisher<sensor_msgs::Range>("tof/front");
    this->createWallFreq(PUBLISH_FREQUENCY_TOF, (ros2::CallbackFunc)publishTof_Front, nullptr, tof_front_pub_);
    // ToF 左
    ros2::Publisher<sensor_msgs::Range>* tof_left_pub_ = this->createPublisher<sensor_msgs::Range>("tof/left");
    this->createWallFreq(PUBLISH_FREQUENCY_TOF, (ros2::CallbackFunc)publishTof_Left, nullptr, tof_left_pub_);
　　（略）
  }
};

今回作成したソフトではPublisherを13個定義して動作することを確認しています。
ros2arduinoのReadmeに説明がありますが、Pub/Subの最大数が定義されているのでこれを超えて拡張するためにはlibrary側の編集が必要です。

#define USER_ROS2_PUBLISHER_MAX       13

メッセージを追加するには

多くの通信は元からあるメッセージで足りたのですが、ToFだけsensor_msgs/Rangeメッセージを使用したくて、メッセージ型の追加も行いました。

• メッセージのIDを追加します

topic_id_number.hというファイルがあるので次のように追加したいメッセージ用にIDを追加します。

  /* sensor_msgs */
  SENSOR_MSGS_IMU_ID,
  SENSOR_MSGS_LASER_SCAN_ID,
  SENSOR_MSGS_BATTERY_STATE_ID,
  SENSOR_MSGS_MAGNETIC_FIELD_ID,
  SENSOR_MSGS_JOINT_STATE_ID,
  SENSOR_MSGS_JOY_ID,
+ SENSOR_MSGS_RANGE_ID,

• メッセージ定義のファイルを作成します

今回はsensor_msgs/RangeのためにRange.hppとして次の内容のファイルを追加しました。手打ちで作成するのはあまりお行儀がよく無さそうなんですが、そのあたりは別の機会に試そうと思います。

#ifndef _SENSOR_MSGS_RANGE_HPP_
#define _SENSOR_MSGS_RANGE_HPP_

#include "../topic.hpp"

#include "../std_msgs/Header.hpp"

namespace sensor_msgs {

enum RadiationType 
{
  RADIATION_TYPE_ULTRASOUND = 0,
  RADIATION_TYPE_INFRARED = 1
};

class Range : public ros2::Topic<Range>
{
public: 
    std_msgs::Header header;
    uint8_t radiation_type;
    float field_of_view;
    float min_range;
    float max_range;
    float range;

  Range():
    Topic("sensor_msgs::msg::dds_::Range_", "Range", SENSOR_MSGS_RANGE_ID),
    header(),
    radiation_type(0), field_of_view(0), min_range(0), max_range(0), range(0)
  { }

  bool serialize(void* msg_buf, const Range* topic)
  {
    ucdrBuffer* writer = (ucdrBuffer*)msg_buf;
    (void) header.serialize(writer, &topic->header);
    (void) ucdr_serialize_uint8_t(writer, topic->radiation_type);
    (void) ucdr_serialize_float(writer, topic->field_of_view);
    (void) ucdr_serialize_float(writer, topic->min_range);
    (void) ucdr_serialize_float(writer, topic->max_range);
    (void) ucdr_serialize_float(writer, topic->range);

    return !writer->error;
  }

  bool deserialize(void* msg_buf, Range* topic)
  {
    ucdrBuffer* reader = (ucdrBuffer*)msg_buf;
    (void) header.deserialize(reader, &topic->header);
    (void) ucdr_deserialize_uint8_t(reader, &topic->radiation_type);
    (void) ucdr_deserialize_float(reader, &topic->field_of_view);
    (void) ucdr_deserialize_float(reader, &topic->min_range);
    (void) ucdr_deserialize_float(reader, &topic->max_range);
    (void) ucdr_deserialize_float(reader, &topic->range);

    return !reader->error;
  }

  uint32_t size_of_topic(const Range* topic, uint32_t size)
  {
    uint32_t previousSize = size;

    size += header.size_of_topic(&topic->header, size);

    size += ucdr_alignment(size, 1) + 1;
    size += ucdr_alignment(size, 4) + 4;
    size += ucdr_alignment(size, 4) + 4;
    size += ucdr_alignment(size, 4) + 4;
    size += ucdr_alignment(size, 4) + 4;

    return size - previousSize;
  }
};

} // namespace sensor_msgs

#endif // _SENSOR_MSGS_RANGE_HPP_

• メッセージをincludeします

msg_list.hppから作成したメッセージ定義ヘッダをインクルードします。

#include "sensor_msgs/Range.hpp"

以上の作業でメッセージ型を追加することができました。
簡単に編集出来てとてもよくできたシステムだと感じます。

上記で追加したメッセージは次のようにAgent側からSubscribeすることができます。

$ ros2 topic echo /tof/front
header:
  stamp:
    sec: 354
    nanosec: 728000000
  frame_id: tof_front
radiation_type: 1
field_of_view: 0.47123900055885315
min_range: 0.009999999776482582
max_range: 1.0
range: 0.2314072996377945
---

このデータのPublish周期はros2arduino上のタイマで20Hzを指定しているのですが、確認したところ良さそうな値になっていました。

$ ros2 topic hz /tof/front
average rate: 17.114
	min: 0.038s max: 0.153s std dev: 0.02584s window: 18
average rate: 18.447
	min: 0.038s max: 0.153s std dev: 0.01826s window: 38
average rate: 18.911
	min: 0.038s max: 0.153s std dev: 0.01491s window: 58
average rate: 19.151
	min: 0.038s max: 0.153s std dev: 0.01298s window: 78
average rate: 19.106
	min: 0.038s max: 0.153s std dev: 0.01262s window: 97

車輪速度などはなるべく高速に受け取りたいところですが、通信量が多くなると設計値を下回るシーンが増えていくことも確認しています。Publisher数や周期で機体に目的にあったところを探すと良いかと思います。

rqt_plotで表示する

トピックとして受け取れるようになると、当然グラフ表示もできます。マウス開発においては設計と実装が一致するまで追い込む作業がとても長く、ツライのでこのようにタイムラグ無く機体情報を視覚的に確認できるツールというのは強力です。

rviz2で表示する

ROSといえば、Odometry、Range、迷路情報など視覚化できるトピックをrvizで統合表示できるのが強みですね。このように自機の移動経路を表示しながら壁の認識をMarker表示し、ToFで壁との距離を計測できていることが一目で確認できるようになってきました。

調整に使っていて特に、マウスが実際に走行した物理的な距離と、rviz上で移動した距離を比べると、タイヤ径が適切に調整できているか確認できるといった使い方が便利でした。
壁と柱も出せるようになったので迷路を広げずにアルゴリズムをデバッグできる領域も増やせそうです。

走行ログの記録にrosbag2も活用してみると有益そうです。
TFのクセに少し悩まされているのですが、さらに改良していこうと思っています。

まとめ

ROS2を使って開発ツールを整えるアピールを色々と挙げましたが、最も有益なのはROS1と違ってロボット都合で電源をON/OFFしてもホスト側に影響なく通信を再開できることでした。小型のロボットはこれだけでもROS2に乗り換える価値があると感じるのでROS1で十分と思っている方も是非。
ホビーのロボットでも手軽にROS2を使おうよ！という情報発信ができるよう、さらにROS2にチャレンジしていこうと思います。