ROS 2 実践コース｜#E2 いまどこにいて、どう行くか（AMCL と Nav2）

前の週（#E1）で地図ができました。この週は、その地図の上でいまどこにいるかを推定し、そこからどう行くかを決めます。

どちらも、答えが 1 つに決まらない計算です。自己位置は「たぶんこのあたり」という広がりを持ちますし、経路は設定を変えれば別の道が出ます。その「たぶん」を数値で読めるようにするのが、この週です。

この記事の位置

実物のロボットは使いません。いま使っている機体は /odom（走った距離の推定）を出していないので、自己位置推定がそもそも動きません。代わりに、PC の中の turtlebot3 を走らせます。ここで扱う仕組みは、実物でもそのまま同じです。違うのは、車輪の回転から距離が分かるかどうかだけです。

この記事は延長コース Eの 2 回分（E3・E4）です。授業で受ける人は、記録を実習レポート（Word）の「回 E3」「回 E4」のシートに分けて書きます。スクリーンショットは、シートの中の「画面：」の欄（実験の表のすぐ下）に貼ります。延長コースのレポートは、コア（第 1〜12 回）とは別の 1 本にします。ファイルは Google Classroom で配られ、コースの最後に Google Classroom で 1 回だけ出します。期限は最後の実習から 1 週間後の 23:59 です。ひとりで学ぶ人も、予想と結果を同じ形でノートに残しておくと、あとで見返せます。

回 E3　自己位置推定（AMCL）

この回は、シミュレーションを起動して AMCL に初期位置を教え、候補（パーティクル）の数と広がりの数値がどう変わるかを読みます。

# どの機体を使うかを、環境変数で確かめる
# （PC のイメージに TURTLEBOT3_MODEL=waffle が入っているので、ふつうは waffle と出る）
echo $TURTLEBOT3_MODEL

# 何も出なかった（空だった）ときだけ、次の行を打つ。端末を開くたびに確かめる
export TURTLEBOT3_MODEL=waffle

# シミュレーションを起動する（画面なしの gazebo ＋ Nav2 一式）
ros2 launch nav2_bringup tb3_simulation_launch.py \
  headless:=True use_composition:=False

# 機体が湧いたかどうかは、ノードの名前ではなく /odom で確かめる
ros2 topic info /odom
ros2 lifecycle get /amcl

起動できたかどうかを、ノードの名前で判断してはいけません。ros2 node list には、起動から数秒で /amcl が出てきます。しかしその時点ではまだ何もできない状態（unconfigured）で、地図の枠すら作っていません。ros2 lifecycle get /amcl が active になったか、/odom に発行元が 1 つ以上ついたかで確かめます。

立ち上がったら、別の端末で初期位置を教え、推定した位置と粒を見ます。

# 初期位置を教える（RViz の 2D Pose Estimate と同じことを端末から）
ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: map}, pose: {pose: {position: {x: -2.0, y: -0.5, z: 0.0}, \
orientation: {w: 1.0}}}}"

# 推定した位置と、その広がりを見る
ros2 topic echo /amcl_pose --field pose.pose.position
ros2 topic echo /amcl_pose --field pose.covariance

# 粒を数える（--full-length を忘れると 128 で打ち切られる）
ros2 topic echo /particle_cloud --full-length --once | grep -c weight

E3-1　パーティクルの広がりを見る

RViz で見ると、たくさんの小さな矢印が出ます。1 本 1 本が「ここにいるかもしれない」という候補です。この講座の設定では、候補の数は min_particles: 500 から max_particles: 2000 の間でそのつど変わります。数えると、こうなりました。

粒の数そのものが、迷いの大きさです。多いから悪い・少ないから良いではなく、「確かめるほど減り、分からなくなると増える」と読みます。ただし、動かしたあとは 501 個のまま変わらなかったので、E3-2・E3-3 では、迷いの大きさを粒の数ではなく広がりの数値（covariance、共分散）で追います。

ただし、数え方を 2 か所まちがえると、いつ数えても 0 個や 128 個になります。どちらも、粒がそう出ているのではなく、こちらの見方のほうが悪いという例です。ここは実際にやってみる価値があります。

機体を動かすときは、次の 2 行を 1 組として打ちます。1 行目は、前に 0.15 m/s で進む指令を 1 秒に 10 回、3 秒のあいだ送ります（約 0.45 m 進むので、0.25 m を超えます）。2 行目は、速さ 0 の指令を 1 回送って止めます。ここで送る /cmd_vel は、シミュレーションの中の機体にだけ届きます。

# 動かさないと粒は更新されない（0.25 m 動くまで AMCL は何もしない）
timeout 3 ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.15}}"

# 止める。timeout で切るだけでは止まらない（最後の速度が残る）
ros2 topic pub -1 /cmd_vel geometry_msgs/msg/Twist "{}" 

「0 件だったから届いていない」と決めつけないでください。受け取れなかっただけかもしれませんし、相手がまだ出していないだけかもしれません。

E3-2　初期位置をわざと 1 m ずらす

ここでは、正しい位置から 1 m ずらして初期位置を与え、動かしながら戻るかを見ます。

# 本当は (-2.0, -0.5) にいる。x を 1 m ずらして (-1.0, -0.5) だと教える
ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: map}, pose: {pose: {position: {x: -1.0, y: -0.5, z: 0.0}, \
orientation: {w: 1.0}}}}"

# 推定した位置（AMCL の答え）を 1 回だけ読む
ros2 topic echo /amcl_pose --field pose.pose.position --once

# 走った分（シミュレーションでは、これを本当の動きとみなす）を 1 回だけ読む
ros2 topic echo /odom --field pose.pose.position --once

# 広がりを 1 回だけ読む。先頭の数が x の広がり（xx）
ros2 topic echo /amcl_pose --field pose.covariance --once

1. シミュレーションを起動し直してから、上の 1 つ目のコマンドで (-1.0, -0.5) を教えます。機体は本当は出発点 (-2.0, -0.5) にいるので、1 m ずれた位置を教えたことになります。
2. 教えた直後に、推定した位置・/odom・広がりを 1 回ずつ読みます。
3. E3-1 の 2 行（3 秒進めて止める）を 1 回打つたびに、同じ 3 つを読みます。これを 5 回くり返します。
4. 「本当の位置」は、出発点 (-2.0, -0.5) に /odom の x・y を足したものです。「本当の位置とのずれ」は、推定した位置と本当の位置の距離（x の差と y の差を二乗して足し、平方根をとる）です。
5. 「covariance の xx」は、pose.covariance の先頭（0 番目）の数です。

予想：ずらしても戻るでしょうか。戻るとしたら、何を手がかりに戻るのかを考えてから試します。手がかり：上の図の赤い点（レーザーで見た壁）と、地図の壁の重なり方を見てください。AMCL は、この 2 つがよく重なる候補を残します。

E3-3　壊す実験：全く違う場所を教える

ここでは、1 m ではなく部屋の反対側を初期位置として教え、同じように動かして戻ってくるかを見ます。

# 本当は (-2.0, -0.5) にいるのに、(1.0, 1.5) だと教える
ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: map}, pose: {pose: {position: {x: 1.0, y: 1.5, z: 0.0}, \
orientation: {w: 1.0}}}}" 

シミュレーションを起動し直してから、上のコマンドで (1.0, 1.5) を教えます。機体は本当は (-2.0, -0.5) にいます。そのあとは E3-2 と同じく、3 秒ずつ 5 回進め、そのたびにずれと xx を読んで、E3-2 と同じ形の表に書きます。

予想：さっきは戻りました。今度も戻るでしょうか。手がかり：AMCL の候補は、教えた位置のまわりにばらまかれます。本当の位置 (-2.0, -0.5) は、その候補の中に入っているでしょうか。

E3-4　広がりを数値で読む

ここでは、E3-2・E3-3 で読んだ covariance の並び方と、0 に近い値の意味を確かめます。

/amcl_pose には covariance（共分散）という配列が入っています。これが「どれだけ広がっているか」の数値です。6 × 6 の表を 1 列に並べたものなので、対角は 0・7・14・21・28・35 番目です。先頭（0 番目）が x の広がり、7 番目が y の広がり、35 番目が向きの広がりです。

止まっているときに読むと 1e-13 のような、ほとんど 0 の値が並ぶことがあります。これは「すごく自信がある」という意味ではなく、与えられた値をそのまま持っているだけで、まだ何も確かめていない状態です。動かして初めて、その数値に意味が出ます。

見た目と数値が合っているかを確かめてください。RViz で広がって見えるのに数値が小さい、という場面があれば、それは書く価値のある発見です。

この回で撮っておく画面

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

• TZ=Asia/Tokyo date（回の頭）
• パーティクルの広がり（RViz）（実験 E3-1 の欄）
• 1 m ずらした直後（実験 E3-2 の欄）
• 動かしたあとに戻ったところ（実験 E3-2 の欄）
• 全く違う場所を教えたとき（実験 E3-3 の欄）
• /amcl_pose の covariance（実験 E3-4 の欄）

本質の問い（回 E3）

• 自己位置が「合っている」と、外からどうすれば言えるか。自分が読んだ数値で説明しなさい。

回 E4　Nav2 にゴールを与える

この回は、Nav2 にゴールを与えて着くまでを記録し、届かないゴールと設定の違いで、ふるまいと経路がどう変わるかを見ます。

1. E4-1　初期位置を正しく与えてから、ゴールを与えて RViz で見ます。
2. E4-2　着くまでにかかった時間と、途中の distance_remaining（残りの距離）を記録します。
3. E4-3　壊す実験：届かないゴール（壁の中、地図の外）を与えます。返る結果と、そこに至るまでにロボットが何を試したかを書きます。
4. E4-4　inflation_radius を変えて、通る道がどう変わるかを見ます（3 通り）。

E4-1〜E4-2　ゴールを与えて、着くまでを記録する

ここでは、正しい初期位置を教えたあとにゴールを送り、経路と残りの距離を見ます。

シミュレーションを起動し直し、回 E3 の初めと同じコマンド（/initialpose に x: -2.0, y: -0.5）で正しい位置を教えてから、次を打ちます。

# Nav2 にゴールを与える。--feedback で、途中の様子（残りの距離など）も出す
ros2 action send_goal --feedback /navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: map}, pose: {position: {x: 1.5, y: -1.5, z: 0.0}, \
orientation: {w: 1.0}}}}" 

E4-2 では、distance_remaining を 5 回ほど書き写し、そのときの navigation_time も一緒に書きます。最後に Goal finished with status: SUCCEEDED のような行が出たら、着いたということです。そのときの経過時間が「着くまでにかかった時間」です。

予想：残りの距離は、時間に比例してまっすぐ減っていくでしょうか。手がかり：道は曲がっていますし、曲がり角では速さを落とします。

E4-3　壊す実験：届かないゴールを与える

ここでは、届かない場所をゴールにして、Nav2 がどう失敗するかを見ます。

# 届かないゴール（地図の外）を与える
ros2 action send_goal --feedback /navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: map}, pose: {position: {x: 5.0, y: 5.0, z: 0.0}, \
orientation: {w: 1.0}}}}" 

コマンドの形は E4-1 と同じで、ゴールの位置だけを地図の外 (5.0, 5.0) にしています。シミュレーションを起動した端末に流れるログも、一緒に見ておきます。

予想：届かないゴールを送ると、すぐに失敗が返るでしょうか。手がかり：Nav2 には、詰まったときに状況を変えてやり直す仕組み（立て直し）があります。E4-2 の number_of_recoveries は、その回数を数える欄でした。

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

地図の外（5.0, 5.0）に送ったときは ABORTED になりましたが、すぐにあきらめたわけではありません。端末とログを並べて見ると、こういう順番で手を尽くしていました。

その場で回る（spin） → 待つ（wait） → 後ろに下がる（backup） → もう一度その場で回る → あきらめる（ABORTED）

「経路が作れない」ときに、ただ止まるのではなく状況を変えてからもう一度試す作りになっています。この並び方は設定ファイルで決められていて、変えられます。次の #E3 で、その中身を読みます。

E4-4　inflation_radius を変える

ここでは、壁からの離れ方を決める値を 3 通りに変え、経路の点の数・道の長さ・壁との距離を測ります。

inflation_radius は、「壁からどれだけ離れて通るか」を決める値です。大きくすると安全側に寄りますが、狭いところを通れなくなります。小さくすると通れますが、壁すれすれを走ります。

この値は、動かしたまま変えられます。ros2 param set を打つと Set parameter successful と出て、次に作る経路からすぐ効きます。立ち上げ直す必要はありません。元の値に戻せば、経路も元に戻ります。

測るのには、次の小さなプログラムを使います。Nav2 が経路を作るたびに出す /plan（経路の点の並び）を 1 本受け取り、点の数・点と点のあいだの距離の合計（道の長さ）・各点からいちばん近い壁のマスまでの距離の最小値（壁との距離）を出します。壁のマスは /map（#E1 で作った地図）から読みます。ホームに path_check.py という名前で保存します。

#!/usr/bin/env python3
"""次に出る経路（/plan）を 1 本受け取り、点の数・道の長さ・壁との距離（最小）を出す。

使い方：python3 path_check.py 0.90   （数字は、表に書くときの見出し）
"""
import math
import sys

import rclpy
from nav_msgs.msg import OccupancyGrid, Path
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile


class PathCheck(Node):
    def __init__(self, label):
        super().__init__("path_check")
        self.label = label
        self.walls = None
        self.done = False
        # /map は「あとから来た人にも最後の 1 件を配る」設定で出ているので、受ける側もそろえる
        latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self.create_subscription(OccupancyGrid, "/map", self.on_map, latched)
        self.create_subscription(Path, "/plan", self.on_plan, 10)

    def on_map(self, m):
        info = m.info
        self.walls = []
        for i, v in enumerate(m.data):
            if v >= 65:  # 65 以上のマスを壁とみなす
                col, row = i % info.width, i // info.width
                self.walls.append((info.origin.position.x + (col + 0.5) * info.resolution,
                                   info.origin.position.y + (row + 0.5) * info.resolution))
        print(f"地図を受け取りました（壁のマス {len(self.walls)} 個）。経路を待っています")

    def on_plan(self, path):
        if self.walls is None or self.done:
            return
        pts = [(ps.pose.position.x, ps.pose.position.y) for ps in path.poses]
        if len(pts) < 2:
            print("経路が空でした。初期位置を教えたか、ゴールが届く場所かを確かめてください")
            self.done = True
            return
        length = sum(math.dist(a, b) for a, b in zip(pts, pts[1:]))
        nearest = min(min(math.dist(q, w) for w in self.walls) for q in pts)
        print(f"{self.label}  点の数 {len(pts)}  道の長さ {length:.3f} m  "
              f"壁との距離（最小） {nearest:.3f} m")
        self.done = True


def main():
    rclpy.init()
    node = PathCheck(sys.argv[1] if len(sys.argv) > 1 else "")
    while rclpy.ok() and not node.done:
        rclpy.spin_once(node, timeout_sec=0.5)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

端末を 2 つ使い、値を 1 つ決めるたびに次の順で打ちます。

1. 端末 1 で python3 path_check.py 0.20 を起動します（数字は表の見出し用）。「経路を待っています」と出たら、そのままにしておきます。
2. 端末 2 で、下のコマンドの 1 つ目（ros2 param set）の最後の数を 0.20 にして打ちます。
3. 端末 2 で、2 つ目のコマンド（compute_path_to_pose）を打ちます。ロボットは動かさず、出発点 (-2.0, -0.5) からゴール (1.5, -1.5) までの経路だけを計算させます。use_start: true は「出発点は、いまいる場所ではなく start に書いた場所を使う」という意味です。
4. 端末 1 に 1 行出て、プログラムが終わります。その行を表に写します。
5. 0.55・0.90 でも同じことをくり返し、最後に 0.55（既定）に戻します。

# 壁からどれだけ離れて通るかを、その場で変える
ros2 param set /global_costmap/global_costmap \
  inflation_layer.inflation_radius 0.90

# ロボットを動かさずに、経路だけを計算させる（出発点とゴールを両方わたす）
ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose \
  "{start: {header: {frame_id: map}, pose: {position: {x: -2.0, y: -0.5, z: 0.0}, \
orientation: {w: 1.0}}}, goal: {header: {frame_id: map}, pose: {position: \
{x: 1.5, y: -1.5, z: 0.0}, orientation: {w: 1.0}}}, use_start: true}" 

「遠回り」は、道の長さを、出発点からゴールまでのまっすぐな距離 3.640 m で割った値です。壁のマスをどう決めるか（ここでは地図の値が 65 以上のマス）で、壁との距離は少し変わります。

予想：0.20・0.55・0.90 と大きくするにつれて、道の長さと壁との距離はどう並ぶでしょうか。手がかり：値を大きくすると、壁のまわりの「通りたくない帯」が太くなります。ただし、部屋の隙間の幅は変わりません。

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

この講座で測った結果です。同じ出発点 (-2.0, -0.5) から同じゴール (1.5, -1.5) まで、まっすぐなら 3.640 m のところを測っています。

inflation_radius	経路の点の数	道の長さ	遠回り	壁との距離（最小）
0.20	136	3.939 m	1.08 倍	0.215 m
0.55（既定）	178	4.688 m	1.29 倍	0.352 m
0.90	172	4.500 m	1.24 倍	0.353 m

注目してほしいのは、どちらの列も順番どおりに増えていないことです。道の長さは 0.90 のほうが 0.55 より短く、壁との距離は 0.55 と 0.90 がほとんど同じです（0.352 m と 0.353 m、差は 0.001 m）。はっきり変わったのは 0.20 と 0.55 のあいだだけでした。

なぜそうなるのか。ここからは推測です。この地図では通れる隙間の幅が決まっていて、0.55 の時点ですでに隙間の真ん中あたりを通っています。そこからさらに値を大きくしても、道をずらす余地がもう無い——と考えると、つじつまが合います。確かめたければ、もっと広い場所で同じ 3 通りを測ってください。広い場所でも 0.55 と 0.90 が同じなら、この推測は外れです。

「安全にすると遠回りになる」「値を上げるほど壁から離れる」と思い込んで表を見ると、読み間違えます。測った値が予想と違ったときは、測り方を疑う前に、予想のほうを疑ってください。

もうひとつ大事なことがあります。この表は、測り直すと少し変わります。同じ 0.55 を別の日に測ったときは、点 185 個・道のり 4.713 m・壁まで最小 0.319 m でした。点の数で 7 個、壁との距離で 0.03 m ずれています。1 回しか測っていない数字を「その設定の値」として書かないでください。レポートには、何回測ってどれくらいばらついたのかも書きます。

この回で撮っておく画面

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

• TZ=Asia/Tokyo date（回の頭）
• ゴールを与えた RViz（実験 E4-1 の欄）
• --feedback の distance_remaining と、着いたときの結果（端末）（実験 E4-2 の欄）
• 届かないゴールの結果（端末）（実験 E4-3 の欄）
• inflation_radius を変えた経路（実験 E4-4 の欄）
• ros2 param set が効いた画面と path_check.py の出力（実験 E4-4 の欄）

つまずきやすい点

まとめ

次の #E3 では、詰まったときのふるまいを読み、最後に床で走らせます。