Zenn
⚽

Raspberry Pi 4を搭載したロボット11台をROS 2で動かした話

2021/12/07に公開
Raspberry Pi
ROS2
robocup
tech

こんにちは。と雷です。私はRoots(ルーツ) ( Twitter, 公式Webサイト)というロボカップサッカーの社会人チームに所属しています。

先日開催されたロボカップアジア・パシフィック2021あいちにて、ROS 2を使ってロボットをたくさん動かしたので、その報告記事を書きます。

「何が起きたか」は書いていますが、「何故そうなったか」は書いていませんのでご理解ください。

ロボカッパー向けのコンテンツ

まずはロボカッパー向けのご報告です。ROS 2ユーザの皆様、少々お待ちください。

どんなロボットを動かしたか

FrootsPi(フルーツパイ) というRaspberry Pi 4を搭載したロボットを動かしました。

https://twitter.com/roots_ssl/status/1463730265503657984?s=20

ロボカップSSL（Small Size League）に出場するロボットは、サッカーをするために次の機能を持っています。

  • 【走る】オムニホイールによる全方位移動機能
  • 【操る】ローラによるボール保持機能
  • 【蹴る】ソレノイドによるキック機能

これらの機能を組み合わせると、次の動画のようにロボットが動きます（動画は2020年のMaker Faire Tokyoでの展示風景）

https://twitter.com/roots_ssl/status/1312589124134596608?s=20

走る、蹴る以外に、外部のコンピュータから動作指令を受信するための通信機能も持っています。

ここまでの機能はRootsのロボットに限らず、ロボカップSSLに出場する全てのロボットに搭載されています。

RootsのロボットにはRaspberry Piが搭載されているので、OSとソフトウェアについてざっくりと説明します。

Raspberry PiのSDカードにはUbuntu Server 20.04というOSをインストールしています。
このOS内に、ROS 2 Foxyというミドルウェアをインストールしてます。

そして、FrootsPiというROS 2パッケージもインストールしてます（詳細はのちほど）

どれくらいの台数を動かしたか

11台動かしました。（作ったのは12台です。）

https://twitter.com/roots_ssl/status/1463766457083252736?s=20

実際に動いている様子がこちらです。（0:31~ 青色マーカが付いたロボットがFrootsPiです）

YouTubeのvideoIDが不正ですhttps://youtu.be/mt6SpIOYs3U?t=31

どこで動かしたか

Aichi Sky Expo (愛知県国際展示場) で動かしました。

Aichi Sky Expo

ここがSSLの会場です。ひろい。フィールドサイズは12m x 9mです。

無人のロボカップSSLフィールド

誰が動かしたか

私のノートパソコン、IdeaPad Gaming 350i 15.6型がロボットを動かしました。

CPU:Core i7(第10世代)、メモリ:16GB、SSD:256GB、HDD:1TB、GPU:GeForce GTX 1650 Tiを搭載して13万円くらいだったので、コスパ良かったです。重たいのが難点。

あらかじめインストールされているWindows 10に加え、Xubuntu 20.04というUbuntu派生のOSをインストールしています。こちらは軽量なOS。パソコンは重たいけど。
もちろん、ROS 2 Foxyもインストールしてます。

この写真は会場近くのホテルで撮影したものです。よくわかりませんが、ロボットと安定化電源も置いてあります。いいホテルですね。

私のノートパソコン

どうやって動かしたか

CON-SAI(コンサイ)というROS 2パッケージでロボットを動かしました。
いわゆるサッカー戦略ソフトです。

ロボットがボールをパスしたり、シュートしたり、きれいに整列できるのもこのソフトウェアのおかげです。

ロボカップSSLでは参加チームそれぞれが独自の戦略ソフトを開発しています。Rootsはそれを公開しています。つまり、このソフトを使えば誰でもロボカップSSLに参加できちゃうのです。

https://twitter.com/roots_ssl/status/1464090285672075270?s=20

ちなみに、Rootsは今大会準優勝でした。みんなで準優勝になろう！

https://twitter.com/roots_ssl/status/1464808555534381068?s=20

ROS 2ユーザ向けのコンテンツ

おまたせしました。ここからがROS 2に関わる話です。

どうやって動かしたか

CON-SAIの中身についてもう少し詳しく話します。
CON-SAIは私のノートパソコンの中にインストールされています。このノートパソコンから、どうやって11台のロボット（FrootsPi）に指示を出したのかを説明していきます。

CON-SAIの設計書からROS 2ノードの構成図を抜き取り、ここに貼り付けます。

CON-SAI構成図

いろいろ書いてますが、注目するところは一番右端です。
右端にある赤色の四角がロボット（FrootsPi）です。各ロボットでは、FrootsPiパッケージのROS 2ノードが起動しています。
ただ、このままだと同じ名前のノードがネットワーク内に存在してしまうため、それぞれにrobot*というネームスペースを与え、ノードやトピックの干渉を防いでいます。

同じROS 2ネットワーク内にCON-SAIと各ロボットのノード（FrootsPi）が共存している、と覚えて次に行きましょう。

CON-SAIはGitHubに公開しているので、よかったらソースコードも参照してください。
https://github.com/SSL-Roots/consai_ros2

誰が動かしたか

何度も言いますが、11台のロボットを動かしたのはノートパソコンIdeaPad Gamingです。ありがとう。

が、流石にあの巨大なフィールド上でパソコンとRaspberry Piを通信させるのは難しいので、別途Wi-Fiルータを用意し、通信を助けてもらってます。

使用したのはtp-linkのArcher C7というルータです。
通信に使用した帯域は5GHz帯です（ありがとうRaspberry Pi 4）。

ロボカップ会場では様々なロボットたちがいろんな電波を飛ばしまくるので、会場は大変なでんぱ祭りになってます。
そのため、ロボカップ運営様が、各競技会場ごとに使用できる無線帯域とチャネルを設定しています。ありがとう大会運営者様。
よって、ロボットが全く動かないという状況は避けられています。（完璧に動くとは言っていない）

どこで動かしたか

12m x 9mのフィールドでロボットを動かしました。
対角線上では15 mも離れたロボットに指令を送らなければなりません。

いつもはパソコン内部でROS トピックを出力していますが、15 m離れたロボットに対してトピックを飛ばしたのは今回が初めてです。

確実に遅延やデータ抜けが発生していますが、それを計測していませんでした。次の大会ではログを取りたいです。

どれくらいの台数を動かしたか

11台のロボットを同時に動かしました。
次の動画はロボットがタイヤを回してLEDを光らせている様子です。かっこいい。
（このとき、電波空間にはROSトピックが飛びまくっていると考えると、おそろしい）

https://twitter.com/chmod_x_akasit/status/1463853869004300288?s=20

各ロボットにはRobotCommandというトピックを100Hzで送信しています。
先程のネームスペースを踏まえると、robot0/commandやrobot1/commandのようなトピックがネットワーク内を飛び交っているわけです。

このトピックにはロボットのID、チームカラー（青 or 黄）、走行速度（x,y方向、θ回転）、キック速度（m/s）、ドリブルパワー（0.0 ~ 1.0）がセットされています。

# FrootsPiへの動作指令値

uint32 robot_id
bool team_is_yellow

# m/s
float64 velocity_x
float64 velocity_y
# rad/s
float64 velocity_theta

# m/s
float64 kick_power
bool chip_kick
# 0.0 ~ 1.0
float64 dribble_power

ROS界隈ではロボット制御にros2_controlというパッケージを使ったり、その動作指令値にTwist型のcmd_velトピックを用いることが多いですが、
CON-SAIとFrootsPiではros2_controlもcmd_velトピックも使用していません。
今後は他のパッケージと仲良くなれるように更新したいですね。

どんなロボットを動かしたか

FrootsPiというRaspberry Pi 4を搭載したロボットを動かしました。

次の動画の1:25~、黄色のマーカを付けたロボットがFrootsPiです。
YouTubeのvideoIDが不正ですhttps://youtu.be/B8C0IHzqeIc?t=85

Raspberry Pi、ということで、どうにかしてROS 2からGPIOピンを制御しなければなりません。
そこで使用したのがpigpioライブラリです。

そのpigpioをふんだんに使用して、ごりごりに実装したノードがdriver_component.cppです。

実装内容をチラ見せします。FrootsPiの実装紹介だけで、記事が何個も書けそうです。。。

void Driver::on_discharge_kicker_timer()
{
  // キッカーコンデンサ放電用の定期実行関数
  // タイマーからこの関数が呼ばれるたびに弱キックしてコンデンサの電荷を放電する
  const int TIME_FOR_DISCHARGE_KICK = 1;
  const int NUM_OF_DISCHARGE_KICK = 30;

  // 充電許可フラグをオフにする
  gpio_write(pi_, GPIO_KICK_ENABLE_CHARGE, PI_LOW);
  enable_kicker_charging_ = false;

  // 威力の弱いキック
  gpio_write(pi_, GPIO_KICK_STRAIGHT, PI_HIGH);
  rclcpp::sleep_for(std::chrono::milliseconds(TIME_FOR_DISCHARGE_KICK));
  gpio_write(pi_, GPIO_KICK_STRAIGHT, PI_LOW);

  // 一定回数キックしたらタイマーをオフする
  discharge_kick_count_++;
  if (discharge_kick_count_ > NUM_OF_DISCHARGE_KICK) {
    discharge_kicker_timer_->cancel();
    discharge_kick_count_ = 0;
  }
}

FrootsPiもGitHubに公開しています。いつか、解説記事書きたいですね。。。
https://github.com/SSL-Roots/FrootsPi

おまけ（ネームスペースの与え方）

さて、各FrootsPiにはrobot*というネームスペースを与えていると書きましたが、それをどう実現してるのかを紹介して、終わりにします。

まず、FrootsPiの各ノードを起動するrobot.launch.pyを紹介します。

robot.launch.pyはlaunch引数にidを設定しており、
$ ros2 launch frootspi_examples robot.launch.py id:=0と起動することで、各ノードがrobot0/ネームスペース以下に出現します。

launchファイルの中で重要なのがPushRosNamespace()とComposableNodeContainerのネームスペースです。

PushRosNamespace()は、それを実行するだけで各ノードにネームスペースを与えられるのでとても便利です。
ただ、ComposableNodeContainerにはネームスペースが与えられないようなので（不具合？要検証）、個別にネームスペースを与えています。

    declare_arg_robot_id = DeclareLaunchArgument(
        'id', default_value='11',
        description=('Set own ID.')
    )
    push_ns = PushRosNamespace(['robot', LaunchConfiguration('id')])
    container = ComposableNodeContainer(
        name='frootspi_container',
        namespace=['robot', LaunchConfiguration('id')],
        package='rclcpp_components',
        executable='component_container',  # component_container_mtはmulti threads
        sigterm_timeout='20',  # 終了時の放電時間だけCtrl+C入力後の猶予を設ける
        composable_node_descriptions=[
            ComposableNode(
# 省略
return LaunchDescription([
        declare_arg_robot_id,
        push_ns,

robot*/の設定方法は分かりました。launchファイル実行時に引数idを与えればよいのです。
しかし、そもそも各ロボットにはどうやってIDを与えているのでしょうか？
答え：Raspberry Pi 1台ずつ、launchファイルを編集し、IDをハードコーディング

答え：IPアドレスを使用しました。

さきほど登場したWi-Fiルータには、各Raspberry Piの固定IPアドレス（例：192.168.0.100）が設定されており、IPアドレスの下2桁をロボットのIDとして設定しています。

Raspberry Piが起動するときにrobot.launch.pyが自動実行されるように設定しています。
そのスクリプトファイル robot_launch.shがこちらです。
このスクリプトファイル内で、IPアドレスの取得し、launchファイルの実行しています。

#!/bin/bash

ENVFILE=/home/ubuntu/ros2_ws/install/setup.bash
ROS_DOMAIN_ID_VALUE=22

if [ -f ${ENVFILE} ]; then
    #環境変数読み込み
    echo "Loading ROS2 Env..."
    source ${ENVFILE}
    export ROS_DOMAIN_ID=${ROS_DOMAIN_ID_VALUE}

    # IPアドレスからロボットIDを取得
    # IPアドレスの下2ケタがIDと対応している
    IFACE="wlan0"
    IP_ADDRESS=$(/sbin/ip -f inet -o addr show "${IFACE}" | cut -d\  -f 7 | cut -d/ -f 1)
    ROBOT_ID=`echo ${IP_ADDRESS: -2:2} | sed -e 's/^0//g'`

    echo "ROS2 Launching..."
    #roslaunch実行
    exec ros2 launch frootspi_examples robot.launch.py id:=${ROBOT_ID}
else
    echo "There is no ${ENVFILE}"
fi

まとめと反省点

動いた報告ばかりの記事になりましたが、実際には動いてないことが多かったです。

最後に反省点をまとめるので、どうか、これからもRootsとRootsのソフトウェア（CON-SAI、FrootsPi）をよろしくお願い致します。

また、ロボカップSSLに興味を持った方！私(と雷)かRootsのtwitterアカウントにコメントいただければ反応します！SSLに参加するための情報を提供します！

それでは。

反省点

  • FrootsPiの自動起動に失敗する。どうやらノードのネームスペースがrobot0/robot0/*となる場合があるらしい
  • CON-SAI - FrootsPI間のトピック通信にて、DDSが正しく設定されていない。多分、いや、確実にTCP通信している。UDP通信すべき。（要調査）
  • 大会中一度もrosbagを実行していない
  • CON-SAI、FrootsPi共にREADMEがメンテナンスされていない。チュートリアルが少ない。（大会終わって１週間しか経ってないから許してください）
  • 大会中一度もrosbagでログを取っていない
  • FrootsPiのハードウェアノードはライフサイクルノードとして実装しているが、Configure & Activateしかしていない。もっとDeactivateやCleanupしてあげたい
  • rosbag!!!

Discussion