ROS 2 実践コース｜#11 センサーから行動へ（scan_avoid の構造を読む）

ここまでで、/scan の読み方（#09・#10）と、速度指令の送り方（#08）を別々に確かめました。この回は、その 2 つをつないだノード scan_avoid を扱います。前が空いていれば進み、障害物が近づいたら空いている側へ曲がるノードです。

ねらいは、コードを読むことではありません。入口と出口、パラメータ、状態の移り変わり、止まる条件の 4 つで、ノードの中身を言い当てられるようになることです。この 4 つは、次の回から自分で設計するときに、そのまま書く項目になります。

1. ノードの入口・出口・パラメータを、ツールで調べる（実験 1）
2. 状態の移り変わりを読む（実験 2）
3. 記録データで動かし、ロボットに出さずに速度指令を見る（実験 3）
4. 速度指令を数字で確かめてから、床で止まる距離を測る（実験 4）
5. /scan を途絶えさせ、止まることを確かめる（実験 5）

どの実験も、コマンドを打つ前に「どうなるか」の予想をノートに書いてください。何を予想すればよいかは、次の表のとおりです。各実験の結果の画面と説明は、「結果を見る」をクリックすると開きます。予想を書き、自分で実行してから開いてください。

予想は外れてかまいません。点になるのは、予想と結果が違ったときに「なぜ違ったか」を考えて書くところです。分からないときも空欄にせず、「分からない」と書いて、手がかりにしたことを 1 つ書きます。

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

この回で撮っておく画面

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

• TZ=Asia/Tokyo date（回の頭）と、実験 4 でつないだ直後の robot-status
• ros2 node info /scan_avoid と ros2 param list
• 記録データで、前進から旋回に切り替わったときの ros2 topic echo /cmd_vel_test
• 机の上で確かめた /cmd_vel_test の値（forward_speed を変えた前後）
• 前進をやめて旋回を始めた位置と箱の間をメジャーで測っているところ（写真）
• robot-disconnect した直後の端末

この回で使うもの

ロボットを床で歩かせるのは、授業では先生が「始めてよい」と言ってからです。実験 3 まではロボットを使いません。実験 4 は、机の上で速度指令の向きを数字で確かめてから、床で動かします。

床で測る数値の欄は、自分で測った値を書くためのものです。

実験 1　ノードの入口・出口・パラメータを調べる

この実験は、次の順に進めます。

1. AI に、scan_avoid の入力・出力・パラメータ・止まる条件を表にしてもらう
2. ロボットにつながずに scan_avoid を起動する
3. ros2 node info と ros2 param list で、実物の入口・出口・パラメータを見る
4. AI の表を 1 行ずつツールの結果と突き合わせ、違っていた行を書く

1-1. AI に表を作らせる

AI に表を作らせてかまいません。ただし、scan_avoid はこの講義のために作ったノードなので、AI は中身を知りません。AI はそれらしい表を作りますが、推測がまざります。その表が合っているかどうかは、AI ではなくツールで確かめます。次のプロンプトで頼み、返ってきた表をレポートに貼ります。

ROS 2 Humble の sakigake_samples というパッケージに入っている scan_avoid というノードについて、
次の 4 つを表にしてください。
・入力（受け取るトピックの名前と型）
・出力（出すトピックの名前と型）
・パラメータ（名前と既定値）
・止まる条件（どうなったら速度 0 を出すか）
知らないことや推測で書いたところは、推測だと分かるように書いてください。

1-2. 起動して、ツールで調べる

ロボットにはまだつなぎません。出口の cmd_vel を /cmd_vel_test につなぎ替えて起動します。-r cmd_vel:=/cmd_vel_test は #06 で使った remap（名前のつなぎ替え）です。ロボットが聞いている /cmd_vel とは別の名前にしておくので、このあとロボットにつないでも、このノードの指令ではロボットは動きません。

# ロボットにはまだつながない
ros2 run sakigake_samples scan_avoid --ros-args -r cmd_vel:=/cmd_vel_test

別の端末で、次の 2 つを実行します。node info はノードの入口（Subscribers）と出口（Publishers）を、param list はパラメータの名前を一覧にします。

ros2 node info /scan_avoid
ros2 param list /scan_avoid

パラメータの既定値は、ros2 param get で 1 つずつ見られます。AI の表に書かれた既定値を、この形で確かめます。

ros2 param get /scan_avoid stop_distance

1-3. AI の表と突き合わせる

AI の表の行ごとに、次のコマンドの結果と比べ、合っていれば ○、違っていれば × を付けます。× の行は、AI が何と書き、実物は何だったかを書きます。

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

node info の Subscribers に /scan、Publishers に /cmd_vel_test が出ます。-r cmd_vel:=/cmd_vel_test でつなぎ替えたので、出口の名前が変わっています。

パラメータの名前・既定値・意味は次のとおりです。

パラメータ	既定値	意味
forward_speed	0.3	前進の速さ [m/s]
turn_speed	0.8	旋回の速さ [rad/s]
stop_distance	0.35	前方がこれより近いと、旋回を始める [m]
clear_distance	0.55	前方がこれより空いたら、前進に戻る [m]
front_angle_deg	0.0	ロボットの正面が、LiDAR の何度の向きか
half_width_deg	30.0	前方として見る扇形の、半分の幅 [度]
scan_timeout	1.0	/scan が途絶えてから止まるまでの時間 [s]

ソースの中で、パラメータを宣言しているところです。

self.declare_parameter('forward_speed', 0.3)     # 前進の速さ [m/s]
self.declare_parameter('turn_speed', 0.8)        # 旋回の速さ [rad/s]
self.declare_parameter('stop_distance', 0.35)    # これより近いと旋回を始める [m]
self.declare_parameter('clear_distance', 0.55)   # これより空いたら前進に戻る [m]
self.declare_parameter('front_angle_deg', 0.0)   # ロボットの正面が LiDAR の何度か
self.declare_parameter('half_width_deg', 30.0)   # 前方として見る扇形の半分の幅
self.declare_parameter('scan_timeout', 1.0)      # /scan が途絶えたら止まるまでの時間 [s]

front_angle_deg は、#09 の実験 5 で自分が確かめた値と同じ意味です。ロボットの正面が LiDAR の 0 度でない場合は、ここを変えます。確かめ終わったら、scan_avoid を Ctrl+C で止めます。

実験 2　状態の移り変わりを読む

このノードには状態が 2 つあります。前進と旋回です。移り方は次の図のとおりです。

しきい値とは、「ここを超えたら動きを変える」と決めておく境目の値のことです。

ここで大事なのは、近づく向きと離れる向きで、しきい値が違うことです。stop_distance（0.35 m）で旋回を始め、clear_distance（0.55 m）まで空くまで旋回を続けます。

考える：しきい値が 1 つだったらどうなるか

しきい値を 0.35 m の 1 つだけにしたとします。前方が 0.34 m になって旋回を始めると、向きが少し変わって前方の距離も少し変わります。そのとき何が起きるかを、「0.34 m → 旋回 → …」のように 1 歩ずつ追って書いてください。

パラメータは、起動するときの -p 名前:=値 のほかに、動いている途中でも ros2 param set で変えられます（例：ros2 param set /scan_avoid stop_distance 0.4）。ただし clear_distance を stop_distance より小さくすると、旋回に入ったその周期のうちに前進へ戻ってしまい、実験 2 で考えた「小刻みな入れ替わり」に戻ります。

実験 3　記録データで動かす（ロボットなし）

#10 で記録した room_scan を再生し、そこへ scan_avoid をつなぎます。出口は /cmd_vel_test のままにしておくので、ロボットにつないでいても動きません。ただし、#10 と同じく、記録を再生するときは robot-disconnect で切り離しておきます（実物の /scan と混ざらないように）。端末を 3 つ使います。

1. 端末 1：記録をくり返し再生する
2. 端末 2：scan_avoid を、出口を /cmd_vel_test にして起動する
3. 端末 3：/cmd_vel_test に出てくる速度指令を見る

端末 2 のコマンドの行の終わりにある \ は、「1 つのコマンドを次の行に続けて書く」という意味です。そのままコピーして貼れば 1 つのコマンドとして動きます。

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

# 端末 2：記録の /scan を読ませる。出口はロボットに届かない名前にする
ros2 run sakigake_samples scan_avoid --ros-args \
  -p front_angle_deg:=0.0 -p stop_distance:=0.5 -r cmd_vel:=/cmd_vel_test

front_angle_deg:=0.0 の 0.0 は、#09 の実験 5 で確かめた自分の値に置き換えます（ずれが無ければ 0.0 のまま）。このあとのコマンドも同じです。

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

ここで stop_distance を既定の 0.35 m から 0.5 に上げているのには理由があります。記録の中で前方がいちばん近づく距離が 0.35 m より遠いと、既定の値では旋回に入りません。自分の記録で前方がいちばん近づく距離は、#10 の scan_monitor の「前」で先に確かめておき、stop_distance にはそれより少し大きい値を入れます。

記録は、撮ったときの部屋の形をそのまま持っています。実物で使う値と、記録で試すのに合う値が、同じとは限りません。これは記録で試すときに必ず出てくる話なので、覚えておいてください。

旋回に切り替わると、端末 2 にログが 1 行出ます。その行と、そのときの端末 3 の値を読んで、次の表を書きます。

符号がどちらになるかは、左右のどちらが空いていたかで決まります。左が広ければ正（左回り）、右が広ければ負です。

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

端末 2 のログに「前方 0.42 m に障害物。右へ旋回します」のような行が出ます（距離と向きは自分の記録で変わります）。そのとき端末 3 では、linear.x が 0 になり、angular.z に値が入ります。記録は箱を置いた部屋のものなので、再生のたびに同じ場面で同じ判断をします。

実験 4　速度指令を確かめてから、床で止まる距離を測る

ここからロボットを使います。先生の合図があってから始めてください。

1. robot-connect でつなぎ、robot-status と ros2 topic hz /scan で届いていることを確かめる
2. 机の上に置いたまま、出口を /cmd_vel_test にして起動し、ros2 topic echo で前進の速さと符号を確かめる
3. 先生の合図のあと、囲いの中の床に置き、出口を /cmd_vel にして起動する（remap しない）
4. 1 m 以上手前から箱に向かって歩かせ、前進をやめて旋回を始めた位置と箱の間をメジャーで測る

4-1. つないで、/scan が届いているか確かめる

つないだら robot-status の画面を撮ります（撮る画面 0 の 2 枚目）。ros2 topic hz /scan は、数行出て 5 Hz 前後になっていれば Ctrl+C で止めます。ここで /scan が届いていないと、scan_avoid は「/scan を待っています」と出したまま動きません。

robot-connect
robot-status
ros2 topic hz /scan

4-2. 机の上で、速度指令を数字で確かめる

床に下ろす前に、ロボットを机の上に置いたまま、出口を /cmd_vel_test にして起動します。こうすると、ロボットに届く /cmd_vel には何も出ないので、脚は動きません。床で使う forward_speed の 2 つの値（0.2 と 0.4）で、本当にその速さの指令が出るかを先に数字で見ておきます。

# 端末 1：forward_speed を 0.2 にして起動する（出口は /cmd_vel_test）
ros2 run sakigake_samples scan_avoid --ros-args \
  -p front_angle_deg:=0.0 -p forward_speed:=0.2 -r cmd_vel:=/cmd_vel_test

# 端末 2：出てくる速度指令を見る
ros2 topic echo /cmd_vel_test

前が空いていれば、linear.x が 0.2 になっているはずです。確かめたら端末 1 を Ctrl+C で止め、-p forward_speed:=0.4 に変えてもう一度起動し、linear.x が 0.4 に変わるかを見ます。変える前と後の端末 2 の画面を撮ります。手を LiDAR の前にかざすと、旋回に切り替わって angular.z に値が入るのも確かめられます。

4-3. 床で、旋回を始める距離を測る

ここからは、先生の合図があってからです。ロボットを囲いの中の床に下ろし、出口をつなぎ替えずに起動します。remap を付けないので、指令は /cmd_vel に出て、ロボットが歩きます。4-2 で起動した scan_avoid は Ctrl+C で止めておき、先生の合図のあとに、次のコマンドを自分で打ちます。

ros2 run sakigake_samples scan_avoid --ros-args -p front_angle_deg:=0.0 -p forward_speed:=0.2

止めるときは、このコマンドを打った端末で Ctrl+C を押します。それでも止まらないときは、ロボットの電源スイッチを切ります（#08 と同じです）。ひとりで学ぶ人は、ほかの人に見ていてもらい、すぐ電源を切れるようにしてから歩かせます。

scan_avoid は、前が近くなると前進をやめてその場で旋回します（実験 3 で linear.x が 0 になったところ）。ここで測るのは、前進をやめて旋回を始めた位置での、ロボットと箱の間の距離です。ロボットのどこから測るか（いちばん前に出ている部分か、LiDAR の中心か）を決め、毎回同じところから測って、そのことも書きます。

速さを変えて、3 回ずつ測ります。平均を書いてください。0.4 m/s で測るときは、起動するコマンドの 0.2 を 0.4 に変えます。打つ前に、どちらの速さのほうが箱の近くで旋回を始めるかを予想してください。手がかりは、LiDAR が箱を見てからノードが指令を出し、脚が向きを変え始めるまでにも、ロボットは前へ進み続けていることです。

実験 5　/scan が途絶えたときに止まるか

打つ前に、/scan が届かなくなってから何秒くらいで速度 0 になるかを予想してください。手がかりは、実験 1 で調べたパラメータの一覧です。

ここも、床で歩かせている間に行うので、先生の合図があってからです。実験 4 の 4-3 と同じように床で scan_avoid を歩かせ、歩いている間に、別の端末で次を実行します。Enter を押してから脚が止まるまでの時間を、スマートフォンのストップウォッチなどで測り、何で測ったかも書きます。

robot-disconnect

脚が止まったら、scan_avoid の端末を Ctrl+C で止めます。

記録データでも同じことを確かめられます。ロボットはもう切り離してあるので、そのまま実験 3 の 3 つの端末をもう一度起動し、端末 3 に速度指令が流れているのを見てから、端末 1（ros2 bag play）で Ctrl＋C を押します。このときは端末 2 のログと、端末 3 の /cmd_vel_test を見ます。

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

/scan が届かなくなり、1 秒（scan_timeout）でノードが速度 0 を送ります。

age = (self.get_clock().now() - self.last_scan_time).nanoseconds / 1e9
if age > self.scan_timeout:
    self.get_logger().warn(f'/scan が {age:.1f} 秒届いていないので止まります', throttle_duration_sec=2.0)
    self.turn_direction = 0.0
    self.publisher.publish(cmd)   # cmd は作ったままなので、すべて 0
    return

これは #08 で見た安全の層のうち、いちばん上（自分のノード）の層です。ロボット側にも、指令が途切れたら止まる仕組みがあります。層は重なっていて、どれか 1 つに頼らないのが決まりです。

床で robot-disconnect したとき、脚を止めているのはどれか

床で測った時間を書いてから、次を開いてください。

レポートには、床で測った時間と、それを止めたのがどの層かを分けて書いてください。

本質の問い

授業の最後に、次の問いに答えてください。数値には証拠となる画面を添えます。

• #07 で測った往復の遅れと、#07・#10 で求めた /scan の周期を使い、0.4 m/s で前進しているときに「箱が見えてから旋回が始まるまで」に何 cm 進むかを計算する。式と、使った値を書く。
• その計算と実験 4 の実測を比べ、stop_distance をいくつにすべきかを答える。

つまずきやすい点

まとめ

次の回は、ここまでの 5 週で使ってきた部品を組み合わせて、自分のまとめ課題を設計します。scan_avoid のように 1 つのノードにまとめるのではなく、知覚・判断・行動に分けて設計します。