ROS 2 実践コース｜#13 組み上げて、3 段階で試験する

前の回で、知覚ノードを記録データで確かめました。この回は残りを作って、1 つのシステムとして動かします。

ここでの決まりは 1 つです。前の段階で見つかる間違いを、次の段階に持ち込まない。記録データで直せることを床で見つけると、ロボットを起こし直す手間と、ぶつける危険が増えます。

1. 判断ノードを、手打ちの入力で確かめる
2. 知覚ノードと判断ノードを 1 つの launch ファイルにまとめる（行動はロボットの中）
3. 段階 1：記録データで通しで動かす
4. 段階 2：机の上で、速度指令を数字で確かめる
5. 段階 3：床で動かす（先生の合図のあと）
6. rqt_graph を撮り、#12（第 5 週）で描いた設計のグラフと見比べる

この回の試験では、打つ前に次のことを予想してノートに書いてください。結果の画面と説明は「結果を見る」をクリックすると開きます。予想を書き、自分で実行してから開いてください。

予想は外れてかまいません。外れたときに「なぜ違ったか」を考えると、直す場所が見つかります。分からないときも空欄にせず、「分からない」と書いて、手がかりにしたことを 1 つ書きます。

授業で受ける人は、この回の記録を実習レポート（Word）の「第 11 回」のシートに書きます。スクリーンショットは、シートの中の「画面：」の欄（実験や記録の表のすぐ下）に貼ります。ひとりで学ぶ人も、予想と結果を同じ形でノートに残しておくと、あとで見返せます。

この回で撮っておく画面

動画は撮りません。そのかわり、下の画面を撮って残します。授業で受ける人は、実習レポートの「第 11 回」の、それぞれの「画面：」の欄に貼ります。ひとりで学ぶ人も、同じ画面を残しておくと、あとで値を見直せます。最初の 1 枚は、実習を始める前に端末で TZ=Asia/Tokyo date と打ち、日付と時刻が出た画面です。授業の日に実習したことの記録になります。講義用の環境の時計は世界標準時（UTC）なので、TZ=Asia/Tokyo を付けて日本時間で出します。

• TZ=Asia/Tokyo date と、つないだ直後の robot-status
• 手打ちの ros2 topic pub と ros2 topic echo /cmd_vel_test を並べた画面
• inf を送ったときの ros2 topic echo /cmd_vel_test
• 段階 1（記録データ）で通したときの rqt_graph
• 段階 2（机の上）で、手をかざしたときの ros2 topic echo /cmd_vel_test
• 段階 3（床）で止まった位置と箱の間（写真）

この回で使うもの

床で歩かせるのは、先生が「始めてよい」と言ってからです。段階 1 と 2 が終わっていないものは、床に下ろしません。

1　判断ノードを、手打ちの入力で確かめる

判断ノードは、知覚ノードの出す数字だけを見ています。だから、その数字を手で送れば、知覚ノードが無くても確かめられます。この節は次の順に進めます。

1. AI に判断ノードを作らせ、設計シートと突き合わせてから保存する
2. setup.py に登録してビルドする
3. 動かして、ノードの入口・出口・パラメータが設計どおりかをコマンドで確かめる
4. 距離を手で送り、全部の場合を試す

1-1　AI に作らせる

#12 で書いた判断ノードの設計シートを、次のプロンプトに貼って AI に渡します。

次の設計シートどおりに、ROS 2 Humble で動く Python（rclpy）のノードを書いてください。
パッケージ名は my_task、ファイルは my_task/decider.py です。
設計シートにないトピックやパラメータは足さないでください。
止まる条件は、設計シートに書いたものを全部入れてください。止まるときは、理由が分かるログを 1 行出してください。

（ここに判断ノードの設計シートを貼る）
（入力の形が分かるように、知覚ノードの設計シートも貼る）

返ってきたコードは、保存する前に設計シートと突き合わせます（#04 の「3-2. 設計シートと突き合わせる」と同じです）。とくに、止まる条件の 3 つの場面が、コードのどこで扱われているかを指でたどって確かめます。見つからない場面があれば、AI に「設計シートの止まる条件の○○が入っていない」と伝えて直させます。突き合わせたら、新しいファイルとして開いて貼り、Ctrl+S で保存します。

codium ~/ros2_ws/src/my_task/my_task/decider.py

1-2　登録してビルドする

decider はあとから足したファイルなので、setup.py の console_scripts に自分で登録します（#04 の「3-4」と同じです）。1 行目の最後のカンマを忘れないでください。

codium ~/ros2_ws/src/my_task/setup.py

    entry_points={
        'console_scripts': [
            'front_distance = my_task.front_distance:main',
            'decider = my_task.decider:main',
        ],
    },

cd ~/ros2_ws && colcon build --packages-select my_task && source install/setup.bash

登録を忘れると、ビルドは通っても ros2 run my_task decider で「No executable found」と出ます。

1-3　設計どおりに作られているかを確かめる

# 端末 1：知覚ノードの代わりに、距離を手で送る
ros2 topic pub --rate 5 /front_distance std_msgs/msg/Float32 "{data: 0.3}"

# 端末 2：判断ノードを動かす（出し先はロボットに届かない名前にする）
ros2 run my_task decider --ros-args -r cmd_vel:=/cmd_vel_test

# 端末 3：出てきた速度指令を見る
ros2 topic echo /cmd_vel_test

--rate 5 は 1 秒に 5 回送るという意味で、知覚ノードが出すはずの周期（5 Hz）に合わせています。-r cmd_vel:=/cmd_vel_test は #11 と同じつなぎ替えで、速度指令をロボットに届かない名前に出させます。

動いたら、もう 1 つ端末を開き、ノードの入口・出口・パラメータを設計シートと並べます。

ros2 node info /decider
ros2 param list /decider

見るのは、Subscribers のトピック名と型が知覚ノードの出力と同じか、Publishers が /cmd_vel_test（型は geometry_msgs/msg/Twist）になっているか、パラメータの名前が設計シートと同じか、頼んでいないトピックやパラメータが増えていないか、です。違っていたら、直すのはコードです。

1-4　距離を手で送り、全部の場合を試す

端末 1 を Ctrl+C で止め、送る値を変えて打ち直しながら、設計シートに書いた表のとおりに変わるかを見ます。ここで全部の場合を試しておきます。「設計で決めた動き」の列は、打つ前に自分の設計シートから書き写します。

inf は、次のように送ります。YAML（{data: ...} の書き方）では、無限大を .inf と書きます。

ros2 topic pub --rate 5 /front_distance std_msgs/msg/Float32 "{data: .inf}"

受け取る側で math.isinf() を通していないと、inf は「とても遠い」と解釈されて前進してしまいます。ここで気づけば、床で気づかずにすみます。

最後の行は Ctrl+C で端末 1 を止めるだけです。#11 で scan_avoid に robot-disconnect をしたのと同じことを、自分のノードで確かめています。

結果を見る（予想を書き、自分で実行してから開く）

2　1 つの launch ファイルにまとめる

ノードが増えると、端末をいくつも開いて順番に起動するのは面倒です。#06 でやったように、launch ファイルにまとめます。出し先の名前を引数にしておくのが要点です。この節は次の順に進めます。

1. 例の launch ファイルを、行ごとに読む
2. パッケージの中に launch ファイルを置き、setup.py に場所を登録する
3. ビルドし直して、引数が見えるかを確かめる

2-1　例の launch ファイルを読む

課題 B の例（知覚ノード front_distance と判断ノード decider）です。自分の課題に合わせて、ノードの名前・パラメータの名前と値を、自分の設計シートのものに書き換えて使います。

"""まとめ課題（例）を 1 つのコマンドで起動する。

ros2 launch my_task task.launch.py
ros2 launch my_task task.launch.py cmd_vel_topic:=/cmd_vel_test stop_distance:=0.5
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    arguments = [
        DeclareLaunchArgument('cmd_vel_topic', default_value='/cmd_vel_test',
                              description='速度指令の出し先。ロボットに出すときだけ /cmd_vel にする'),
        DeclareLaunchArgument('stop_distance', default_value='0.35',
                              description='これより近いと止まる [m]'),
    ]
    perception = Node(
        package='my_task',
        executable='front_distance',
        name='front_distance',
        output='screen',
        parameters=[{'front_angle_deg': 0.0, 'half_width_deg': 30.0}],
    )
    decision = Node(
        package='my_task',
        executable='decider',
        name='decider',
        output='screen',
        parameters=[{
            'stop_distance': ParameterValue(LaunchConfiguration('stop_distance'), value_type=float),
            'forward_speed': 0.2,
            'input_timeout': 1.0,
        }],
        remappings=[('cmd_vel', LaunchConfiguration('cmd_vel_topic'))],
    )
    return LaunchDescription(arguments + [perception, decision])

#06 の launch ファイルに無かった書き方が出てくるので、1 つずつ説明します。

cmd_vel_topic の既定を /cmd_vel_test にしてあります。こうしておくと、うっかり ros2 launch しただけではロボットが動きません。床で動かすときだけ、次のように明示します。

ros2 launch my_task task.launch.py cmd_vel_topic:=/cmd_vel

このコマンドはロボットを歩かせます。段階 3 で、先生の合図があってから自分で打ちます。段階 1・2 では、cmd_vel_topic:=/cmd_vel を付けずに起動します。

2-2　パッケージの中に置いて、場所を登録する

#06 では、launch ファイルをファイルの場所で指定して起動しました（ros2 launch ~/ros2_ws/two_squares.launch.py）。上のように ros2 launch my_task task.launch.py という短い形で呼ぶには、launch ファイルをパッケージの中に入れて、場所を登録する必要があります。手順は 3 つです。

1. パッケージの中に launch フォルダを作り、そこに task.launch.py を置く
2. setup.py に足す（下のコード。ファイルの先頭の import 2 行と、data_files の 2 行）
3. ビルドし直して、source する

まず、フォルダを作り、launch ファイルを新しいファイルとして開いて、上の例（を自分の課題に合わせたもの）を貼って保存します。

mkdir -p ~/ros2_ws/src/my_task/launch
codium ~/ros2_ws/src/my_task/launch/task.launch.py

次に setup.py を開いて、2 か所に足します。ros2 pkg create で作った setup.py には import os も from glob import glob も無いので、2 行とも足します。

codium ~/ros2_ws/src/my_task/setup.py

import os                                                # ← 1 つめ：ファイルの先頭に、この 2 行を足す
from glob import glob

setup(
    # ...途中は元のまま...
    data_files=[
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'),
         glob('launch/*.launch.py')),                     # ← 2 つめ：この 2 行を足す
    ],

2 つめは、「launch フォルダの *.launch.py を、インストール先の share/my_task/launch にコピーする」という指示です。ros2 launch my_task ... は、このコピー先を探しに行きます。

2-3　ビルドし直して確かめる

cd ~/ros2_ws && colcon build --packages-select my_task && source install/setup.bash
ros2 launch my_task task.launch.py --show-args

--show-args を付けると、ノードは起動せずに、launch ファイルが受け付ける引数の一覧だけを表示します。cmd_vel_topic と stop_distance が、既定値といっしょに出れば登録できています。

setup.py を変えたので、ビルドし直さないと反映されません。ros2 launch で「file not found」と言われたら、まずこの 3 つのどれかが抜けています。

3　段階 1：記録データで通しで動かす

ロボットは使わず、#10 の記録を再生して、知覚ノードから判断ノードまでを通しで動かします。ロボットにつないでいる人は、先に robot-disconnect で切り離します（実物の /scan と記録が混ざらないように。#11 の実験 3 と同じです）。やることは次の 3 つです。

1. 3 つの端末で、記録の再生・launch・速度指令の表示を動かす
2. stop_distance を変えたときに、判断の切り替わり方がどう変わるかを見る
3. rqt_graph を撮る

# 端末 1：記録を再生する
cd ~/bags
ros2 bag play room_scan --loop

# 端末 2：全部まとめて起動する（出し先は /cmd_vel_test のまま）
ros2 launch my_task task.launch.py stop_distance:=0.5

# 端末 3：速度指令を見る
ros2 topic echo /cmd_vel_test

見るのは、判断が切り替わる場面です。記録は同じ場面を繰り返すので、「箱に近づいた場面で、設計どおりに切り替わるか」を何度でも見られます。

ここでは stop_distance:=0.5 を付けて、止まる距離を既定の 0.35 m から 0.5 m に上げています。なぜ上げる必要があるのかを、はじめに立てた予想と、#11 で確かめた「この記録の中で前方がいちばん近づく距離」から考えてください。考えたら、端末 2 を Ctrl+C で止め、stop_distance:=0.5 を外して起動し直して確かめます。

確かめたら、stop_distance:=0.5 を付けて起動し直し、rqt_graph も一度撮っておきます。表示を「Nodes/Topics (active)」にして更新ボタンを押します（#03 の実験 1）。ノードが線でつながっていて、/cmd_vel_test に出ていることを目で確かめます。

rqt_graph

4　段階 2：机の上で、速度指令を数字で確かめる

足を浮かせる台は使いません。ロボットにはつなぎますが、出し先は既定の /cmd_vel_test のままにして、出ている数字だけを見ます。脚は動かないので、机の上に置いたままで確かめられます。

1. robot-connect でつなぎ、robot-status と ros2 topic hz /scan を確かめる
2. そのまま ros2 launch my_task task.launch.py で起動する（出し先は /cmd_vel_test）
3. ros2 topic echo /cmd_vel_test を開いたまま、ロボットの前に手をかざす
4. linear.x の符号と大きさ、angular.z の符号が、設計どおりかを確かめる

# 端末 1：つないで、届いているかを確かめる
robot-connect
robot-status
ros2 topic hz /scan

# 端末 2：まとめて起動する（引数を付けないので、出し先は /cmd_vel_test、stop_distance は 0.35）
ros2 launch my_task task.launch.py

# 端末 3：速度指令を見る
ros2 topic echo /cmd_vel_test

ros2 topic hz /scan は、周期が出たのを見たら Ctrl+C で止めてかまいません。段階 1 で付けた stop_distance:=0.5 は、記録に合わせた値なので、ここでは付けません。手をかざすときは、LiDAR から 35 cm より近いところと遠いところを行き来させると、切り替わりが見えます。

ここで見るのは向きと大きさです。linear.x は正なら前進、angular.z は正なら左回りです（#05・#11）。前進のはずが負の値になっていないか、旋回の左右が逆になっていないか、forward_speed より大きな値が出ていないかを見ます。床に置いてから気づくと、壁に向かって歩き出すことになります。

この画面を撮って、レポートの第 11 回に貼ります。手をかざしたときと離したときの linear.x の値は、「試験の記録」の表に書きます。

5　段階 3：床で動かす

段階 1 と 2 を通ったものだけを、先生の合図があってから、囲いの中で動かします。歩かせる起動（cmd_vel_topic:=/cmd_vel を付けたもの）は、先生の合図があるまで打ちません。ロボットの周りに人がいないこと、すぐに止められることを確かめてから始めます（#08 の実験 4 と同じです）。ひとりで学ぶ人は、ほかの人に見ていてもらい、すぐ電源を切れるようにしてから動かします。

段階 2 の端末 2（ros2 launch）を Ctrl+C で止め、ロボットを囲いの中の床に下ろします。先生の合図のあとに、端末 2 で次のコマンドを自分で打ちます。止めるときは、この端末で Ctrl+C を押します。

ros2 launch my_task task.launch.py cmd_vel_topic:=/cmd_vel

止まった位置と箱の間は、メジャーで測って写真に撮り、値を「試験の記録」の表に書きます。うまくいかなかったときは、その場で直さずに、どの段階まで戻るかを次の表で決めてから直します。

試験の記録は、段階ごとに残します。レポートにはこの表と同じ形で書きます。動画は撮りません。画面のスクリーンショットと、測った値、直したことを書きます。

6　設計と実物を見比べる

最後に rqt_graph を撮り、#12 で描いた設計のグラフと並べます。違いがあれば、どちらかです。

• 設計を直した：作っている途中で、設計のほうが間違っていると分かって変えた
• 実装が設計からずれた：設計は正しいのに、できたものが違っている

どちらなのかを書き分けます。後者が残っているなら、設計シートを直すか、コードを直すかを決めます。AI に作らせたときに、頼んでいないトピックが増えていることもあります。#04 から繰り返してきた「設計シートとツールの出力を突き合わせる」作業の、最後の回です。

本質の問い

授業の最後に、次の 2 つに答えてください。

• 3 段階のうち、自分がいちばん多く間違いを見つけた段階はどこか。その間違いを床で見つけていたら、何が起きたかを書く。
• 設計のグラフと rqt_graph の違いを 1 つ挙げ、設計を直したのか、実装がずれたのかを書く。

つまずきやすい点

まとめ

次の回は、作ったものを発表し、後半は自分の PC のシミュレーションで SLAM と Nav2 を動かします。