ROS 2 実践コース|#09 座標と時間を調べる(QoS・TF・タイムスタンプ)

前の回の最後に、ロボットに正方形を歩かせると、戻ってきた位置がずれることを確かめました。ロボットが「どこに何があるか」を扱うには、データに 2 つの情報が欠かせません。どの座標系で測ったかと、いつ測ったかです。

この回は、その 2 つを /scan(LiDAR の距離データ)で調べます。その前に、データが届くかどうかを決める約束ごと、QoS を試します。

  1. QoS の組み合わせで、届くか届かないかが変わることを確かめる(実験 1、ロボットなし)
  2. 実物の /scan の QoS を読む(実験 2)
  3. LiDAR の座標系をたどり、RViz で点を表示する(実験 3・4・5)
  4. データに付いた時刻と、届くまでの遅れを測る(実験 6)
  5. ミッション:障害物の手前で自分で止まるノードを作り、床で動かす。自分の動きを 1 つ足す

実験 1〜6 は、ロボットを机の上に置いたまま動かしません。最後のミッションで、実験 1〜6 で調べた QoS・座標系・遅れを使って、自分で止まるノードを作り、床で動かします。

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

実験予想すること
1. QoS の組み合わせ送る側と受ける側の Reliability の組み合わせ 4 通り(A〜D)のそれぞれで、届くか届かないか
2. 実物の /scan の QoSLiDAR の /scan は、reliable と best_effort のどちらで送られているか
3. LiDAR の座標系LiDAR は、ロボットの体の基準(base_link)から見て、前・左・上のどちらへ何 cm くらいのところにあるか
5. LiDAR の 0° の向きロボットの正面に置いた箱の点が、RViz で正面(赤い矢印の先)に出るか
6. 時刻と遅れros2 topic delay /scan の平均は、#07 で測った往復の時間(20 ms ほど)より大きいか小さいか
ミッションLiDAR の前に手をかざしたとき、真横へ動かしたとき、Ctrl+C で止めたときに、自分のノードはそれぞれどんな速度指令を出すか(M-6)

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

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

この回で撮っておく画面

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

  • TZ=Asia/Tokyo date と、つないだ直後の robot-status
  • QoS の組み合わせで届かなかったときの、受ける側の端末
  • ros2 topic info -v /scan(Reliability・History・Durability)
  • ros2 run tf2_ros tf2_echo base_link laser_frame
  • ros2 run tf2_tools view_frames で作った TF の木
  • 箱を正面に置いたときの RViz
  • ros2 topic delay /scan の平均・最小・最大

この回で使うもの

もの使うところ
講義用の環境(#00 で準備したもの)すべての実験
ロボット(机の上に置いたまま動かさない)実験 2〜6
段ボール箱など、LiDAR に映る箱 1 つ実験 5、ミッション
囲い(歩行エリア)ミッション
設計シート(#04 で使った白紙)ミッション
見守り役の相手(隣の学生)ミッション

実験 1 QoS:名前も型も合っているのに届かない

QoS(Quality of Service)は、トピックをどう届けるかの約束ごとです。いちばんよく使うのが Reliability(信頼性)で、次の 2 種類があります。

Reliability届け方
reliable届かなかったら送り直す。確実に届くが、送り直す分だけ遅れることがある
best_effort送りっぱなし。届かなかったものは捨てる

送る側と受ける側は、それぞれ自分の QoS を決めます。組み合わせによって届くかどうかを、ロボットを使わずに PC の中だけで試します。この実験は、次の順に進めます。

  1. 4 通りの組み合わせについて、届くか届かないかを予想して表に書く
  2. A と B を試す(送る側 1 つに、受ける側 2 つを同時につなぐ)
  3. C と D を試す(送る側を reliable に替える)
  4. 結果を表に書き、予想と違ったところの理由を考える

予想する

次の表の「予想」の欄を、自分のノートかレポートに書きます。手がかりは、上の表の「届け方」です。reliable の受ける側は「届かなかったら送り直してほしい」と求めます。送る側がそれに応えられるかどうかを、1 行ずつ考えてください。

番号送る側受ける側予想(届く? 届かない?)結果
Abest_effortreliable
Bbest_effortbest_effort
Creliablebest_effort
Dreliablereliable

A と B を試す

端末を 3 つ開き、上から順に実行します。--qos-reliability で、そのコマンドの Reliability を指定します。

# 端末 1(送る側:best_effort で 1 秒に 2 回送る)
ros2 topic pub --rate 2 --qos-reliability best_effort /qos_test std_msgs/msg/String "{data: 'hello'}"
# 端末 2(受ける側:reliable)
ros2 topic echo --qos-reliability reliable /qos_test
# 端末 3(受ける側:best_effort)
ros2 topic echo --qos-reliability best_effort /qos_test

端末 2 と端末 3 に何が出るかを見て、表の A(端末 2)と B(端末 3)の「結果」を書きます。何も出ないときは、黄色の警告が出ていないかも見てください。

結果を見る(予想を書き、自分で実行してから開く)
上:best_effort で送り続けている。中:reliable で受ける端末には何も届かず、黄色の警告が出た。下:best_effort で受ける端末には hello が届いている
上:best_effort で送り続けている。中:reliable で受ける端末には何も届かず、黄色の警告が出た。下:best_effort で受ける端末には hello が届いている

端末 2 の警告は「新しい送り手を見つけたが、QoS が合わない。この送り手からは何も受け取らない。合わなかったのは RELIABILITY」という意味です。トピック名 /qos_test も型 std_msgs/msg/String も合っているのに、1 つも届きません。

確かめたら、3 つの端末をすべて Ctrl+C で止めます。止めずに次へ進むと、best_effort の送り手が残ったままになり、C と D の結果が読めなくなります。

C と D を試す

今度は送る側を reliable にして、best_effort で受ける端末(C)と、reliable で受ける端末(D)を同時につなぎます。

# 端末 1(送る側:reliable)
ros2 topic pub --rate 2 --qos-reliability reliable /qos_test std_msgs/msg/String "{data: 'hello'}"
# 端末 2(受ける側:best_effort)
ros2 topic echo --qos-reliability best_effort /qos_test
# 端末 3(受ける側:reliable)
ros2 topic echo --qos-reliability reliable /qos_test

端末 2 が C、端末 3 が D の結果です。表の「結果」を書いたら、3 つの端末をすべて Ctrl+C で止めます。

結果を見る(予想を書き、自分で実行してから開く)
送る側が reliable、受ける側が best_effort なら届く
送る側が reliable、受ける側が best_effort なら届く
番号結果理由
A届かない受ける側が「送り直してほしい」と求めているのに、送る側は送り直さない
B届くどちらも送りっぱなしでよいと決めている
C届く送る側は送り直せるが、受ける側はそれを求めていない。求める以上のことをしてくれる相手とは組める
D届く受ける側が求める「送り直し」を、送る側が約束している

受ける側が求めることを、送る側が約束していれば届きます。反対に、受ける側が送る側より多くを求めると届きません。#03 で「名前と型が合わないと届かない」ことを確かめました。QoS も、同じようにトピックの約束ごとの一部です。

実験 2 実物の /scan の QoS を読む

ロボットにつなぎます(手順は #07 の準備と同じです)。つないだら、robot-status でブリッジが動作中かを確かめ、その画面を回の頭の 1 枚として撮ります。ここでつながっていないと、このあとの実験で何も出ません。

robot-connect
robot-status

robot-connect は、IP を付けないと前回つないだ IP につなぎます。OLED の 2 行目の IP が前回と変わっていたら(ホットスポットを立て直したときなど)、robot-connect 192.168.137.134 のように IP を付けて打ちます。

OLED は「PC connected」で LiDAR も回っているのに、ros2 topic hz /scan に何も出ないとき

授業中に Wi-Fi が一度切れると、こうなることがあります。PC とロボットのつながりは戻っても、ロボットの中で /scan を PC へ運ぶ通り道が、切れる前のまま残ってしまうことがあるからです。

OLED の「PC connected」は、PC とロボットの線がつながっているかだけを見ています。データが流れているかどうかは見ていません。LiDAR も、電源が入っていれば回り続けます。「つながっている」「回っている」と「データが届いている」は別のことなので、届いているかは ros2 topic hz で確かめます。

順番やること
1動かしているノードとコマンドを Ctrl+C で止め、robot-disconnect のあと robot-connect でつなぎ直す。もう一度 ros2 topic hz /scan を見て、5 Hz 前後が出れば作業に戻る
2それでも何も出なければ、手を挙げて先生に号機番号を伝える。先生がロボットの中のプログラムを起動し直す(1 分ほどで戻る)

LiDAR のケーブルを抜き差ししたり、ロボットの電源を何度も入れ直したりはしないでください。直るまでのあいだは、#10 で記録したデータを再生すれば、ロボットなしで作業を続けられます(#10 より前の回は、待っているあいだに予想の表を進めます)。

PC から /scan の QoS を読みます。-v は「くわしく表示する」という指定で、これを付けると送り手・受け手ごとの QoS が出ます。予想(reliable か best_effort か)を書いてから実行してください。手がかりは、#04 の設計シートの説明で「センサーの値のように新しいものが次々に来るものは best effort にする」と書いたことです。

ros2 topic info -v /scan

出てくる QoS の行は、次のように読みます。

行意味
Reliability実験 1 で試した届け方。RELIABLE か BEST_EFFORT
History (Depth)まだ渡していないデータを何件までためておくか。KEEP_LAST (10) なら、新しいほうから 10 件だけ残し、古いものは捨てる
Durabilityあとから来た受け手に、前に送ったデータを渡すかどうか。VOLATILE は渡さない(受け手がつないだあとのデータだけが届く)
結果を見る(予想を書き、自分で実行してから開く)
PC から見た /scan。送っているのは zenoh_bridge_pc で、Reliability は RELIABLE
PC から見た /scan。送っているのは zenoh_bridge_pc で、Reliability は RELIABLE

PC から見える送り手はブリッジです。ロボットの中で本当に /scan を送っているノードの QoS は、ロボットの中で調べると、次のようになっていました(2026 年 9 月に測定)。

$ ros2 topic info -v /scan
Type: sensor_msgs/msg/LaserScan

Publisher count: 1

Node name: MS200
Node namespace: /
Topic type: sensor_msgs/msg/LaserScan
Endpoint type: PUBLISHER
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (10)
  Durability: VOLATILE

LiDAR のノード MS200 も RELIABLE で送っています。

考える:センサーのデータを best_effort で受けるのはなぜか

ROS 2 では、センサーのデータは best_effort で送ったり受けたりすることがよくあります。次の回(#10)と第 5 週で使うサンプル scan_monitor も、/scan を best_effort で受けています。この LiDAR は RELIABLE で送っているのに、なぜ受ける側は best_effort にしておくのでしょうか。実験 1 の結果を使って考えてください。

考え方の例(自分の考えを書いてから開く)
  • LiDAR は 1 秒に 10 回、新しい距離を測り直す。0.1 秒前のデータを送り直してもらっても、そのころにはもう新しいデータが届いている。センサーのデータは、古いものを確実に受け取るより、新しいものを早く受け取るほうが役に立つ。
  • 実験 1 の C のとおり、best_effort で受けるノードは、送り手が reliable でも best_effort でも受け取れる。LiDAR を別の機種に替えて送り手の QoS が変わっても、そのまま動く。
  • 反対に reliable で受けるノードを書くと、best_effort で送る LiDAR からは何も届かない(実験 1 の A)。

実験 3 LiDAR の座標系をたどる

ロボットには、体の中心、LiDAR、カメラ、脚の関節など、たくさんの「基準の場所」があります。それぞれの場所に付けた座標系と、座標系どうしの位置関係をまとめて TF と呼びます。ROS 2 では、TF が /tf(脚のように動くもの)と /tf_static(LiDAR の取り付け位置のように動かないもの)の 2 つのトピックで流れています。

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

  1. view_frames で、座標系のつながりを PDF の図にする
  2. PDF を開き、base_link と laser_frame の親子関係を読む
  3. tf2_echo で、LiDAR が体の基準からどこにあるかを数字で見る

座標系のつながりを図にする

作業用のフォルダを作って移動し、view_frames を実行します。5 秒間 TF を集めて、つながりを PDF に書き出します。view_frames は、コマンドを打った場所(いまいるフォルダ)にファイルを作るので、先に専用のフォルダへ移動しておきます。mkdir -p の -p は「すでにあってもエラーにしない」、&& は「左が成功したら、続けて右を実行する」という意味です(#04 の「4. ビルドして動かす」と同じ)。最後の ls で、PDF ができたかを確かめます。

mkdir -p ~/tf_check && cd ~/tf_check
ros2 run tf2_tools view_frames
ls
5 秒待つと frames_(日付).pdf ができる。最後の行はとても長いので、この画面では 100 文字で切って表示している
5 秒待つと frames_(日付).pdf ができる。最後の行はとても長いので、この画面では 100 文字で切って表示している

PDF は、ファイル マネージャーでダブルクリックするか、次のコマンドで開きます。atril は、この環境に入っている PDF を見るためのソフトです。* は「ここは何文字でもよい」という意味で、日付の部分を打たずに済みます。

atril frames_*.pdf
PDF が開いたところ。上から下へ、座標系が枝分かれしてつながっている
PDF が開いたところ。上から下へ、座標系が枝分かれしてつながっている
PDF の上の部分。base_footprint の下に base_link があり、その下に laser_frame がつながっている(赤枠は説明のために付けた)
PDF の上の部分。base_footprint の下に base_link があり、その下に laser_frame がつながっている(赤枠は説明のために付けた)

矢印は「親 → 子」の向きです。base_link(ロボットの体の基準)の子として、laser_frame、camera_link、imu_link、4 本の脚の付け根(lf_hip_link など)がつながっています。

laser_link という似た名前もあります。どれが /scan の座標系かは、データ自身に書いてあります(実験 6 で確かめます)。

atril を端末から開いている間、その端末は次のコマンドを受け付けません。PDF を読み終えたら、atril の窓を閉じてから次へ進みます。

LiDAR は体の中心からどこにあるか

2 つの座標系の位置関係を数字で見ます。打つ前に、机の上のロボットを見て、LiDAR が体のまん中から前・左・上のどちらへ何 cm くらいのところにあるかを予想してください。

tf2_echo は、1 つめの座標系(base_link)から見た 2 つめの座標系(laser_frame)の位置と向きを、くり返し表示し続けます。表示の中の Translation が位置で、3 つの数は x(前)、y(左)、z(上)の順、単位は m です。Rotation が向きです。数字が何回か出たら、Ctrl+C で止めます。

ros2 run tf2_ros tf2_echo base_link laser_frame
結果を見る(予想を書き、自分で実行してから開く)
base_link から見た laser_frame の位置(Translation)と向き(Rotation)
base_link から見た laser_frame の位置(Translation)と向き(Rotation)

[0.000, 0.000, 0.180] なので、LiDAR は体の基準からまっすぐ上に 18 cm の位置にあります。Rotation が回っていない値なので、LiDAR の向きは体と同じとして登録されています。

実験 4 RViz で /scan の点を表示する

RViz を起動します。

rviz2

rviz2 を打った端末は、RViz を閉じるまでほかのコマンドを受け付けません。RViz は実験 5 が終わるまで開いたままにし、実験 6 のコマンドは新しい端末で打ちます。

左の Displays パネルで、次の順に設定します。

  1. Global Options の Fixed Frame を base_link にする(表示の基準にする座標系)。
  2. 左下の Add を押し、By display type の中から RobotModel を選んで OK。Description Topic を /robot_description にする。
  3. 同じように Add から TF を追加する。
  4. Add から LaserScan を追加し、Topic を /scan にする。Topic の左の三角を開き、Reliability Policy を実験 2 で読んだ QoS に合わせる。
LaserScan の Status が Ok になり、ロボットの周りの壁や物が色の付いた点で表示された。中央の小さな矢印の集まりが TF で、赤が x(前)、緑が y(左)、青が z(上)
LaserScan の Status が Ok になり、ロボットの周りの壁や物が色の付いた点で表示された。中央の小さな矢印の集まりが TF で、赤が x(前)、緑が y(左)、青が z(上)

点の色は、光がどれだけ強く返ってきたか(intensity)を表しています。距離ではありません。

LaserScan の Status がエラーになるとき
  • Fixed Frame が base_link になっているか。つづりを 1 文字でも間違えると、点を置く場所が決められない。
  • robot-status でブリッジが動作中か。
  • Reliability Policy を変えた直後は、表示されるまで数秒かかることがある。

RViz の設定は、File → Save Config As で保存しておくと、次の回に rviz2 -d ファイル名 で同じ表示から始められます。

実験 5 LiDAR の 0° はロボットの正面か

ロボットの正面、30 cm ほど先に箱を置き(LiDAR はロボットの背中の上にあるので、LiDAR より背の高い箱を使います。低い箱は点に出ません)、RViz のどこに箱の点が出るかを見ます。

TF の赤い矢印(base_link の x)の向きが、ロボットの正面です。箱の点が赤い矢印の先に出れば、LiDAR の 0° とロボットの正面は一致しています。

箱を置く前に、点が赤い矢印の先に出るかを予想してください。手がかりは、実験 3 の tf2_echo です。Rotation は「LiDAR の向きは体と同じ」として登録されていました。ただし、これは登録した値です。実物の LiDAR が本当にその向きで付いているかは、この実験で初めて分かります。

ずれていたときは、赤い矢印から左回りか右回りか、およそ何度かを読みます。時計の文字盤を上から見るつもりで、赤い矢印を 12 時とし、1 時間分(30°)を目安にするとおおよその角度を読めます。

記録すること結果
箱の点は、赤い矢印に対してどちらに出たか(先・左・右・後ろ)
ずれていたら、およそ何度か

この結果は第 5 週で使います。障害物をよける判断は「前にあるか」で決めるので、前がどちらかを間違えると、よけるつもりでぶつかりに行きます。

実験 6 データに付いている時刻と遅れ

/scan のメッセージには、中身の距離データとは別に、header(見出し)が付いています。RViz は開いたままで、新しい端末を開き、header だけを表示します。

ros2 topic echo /scan --field header

--field は、「メッセージ全部ではなく、この部分だけを表示して」という指定です。/scan は距離データが何百個も並ぶので、そのまま表示すると画面が流れてしまいます。中に入っているものの名前は、ros2 interface show sensor_msgs/msg/LaserScan で確かめられます。

何回分か表示されたら Ctrl+C で止め、同じ端末で PC の今の時刻を、header の stamp と同じ形で表示します。+%s.%N は、「1970 年 1 月 1 日からの秒数(%s)と、その秒の中の 10 億分の 1 秒の数(%N)を、点でつないで出して」という書式の指定です。

date +%s.%N

表示された header の中の frame_id が、実験 3 で tf2_echo に使った座標系と同じかを確かめてください。

結果を見る(予想を書き、自分で実行してから開く)
1 回分の header は stamp(時刻)と frame_id(座標系)。Ctrl+C のあとで date +%s.%N を実行し、PC の今の時刻を同じ形で表示した
1 回分の header は stamp(時刻)と frame_id(座標系)。Ctrl+C のあとで date +%s.%N を実行し、PC の今の時刻を同じ形で表示した
  • frame_id: laser_frame が、実験 3 で「データ自身に書いてある」と言った座標系です。
  • stamp が測った時刻です。sec は 1970 年 1 月 1 日からの秒数、nanosec はその秒の中の 10 億分の 1 秒の単位です。
  • 続けて届いた stamp の差を見ると、0.956 → 1.156 → 1.356 秒のように約 0.2 秒ずつ進んでいます。PC に届く /scan は 5 Hz でした(#07)。

最後の date との差は、Ctrl+C を押してコマンドを打つまでの時間も含むので、遅れの測定には使えません。遅れは次のコマンドで測ります。ros2 topic delay は、「PC が受け取った時刻 − ロボットが付けた stamp」を、届くたびに計算して平均します。何行か表示されたら Ctrl+C で止め、最後の行の平均(average delay)・最小(min)・最大(max)を書きます。

打つ前に、平均が #07 で測った往復の時間(latency_probe の平均、20 ms ほど)より大きいか小さいかを予想してください。手がかりは、stamp が「測った時刻」だということです。測ってから PC に届くまでに、どんな段階を通るかを考えます。

ros2 topic delay /scan
結果を見る(予想を書き、自分で実行してから開く)
PC で受け取った時刻から stamp を引いた値。平均 0.214 秒、最小 0.131 秒、最大 0.518 秒
PC で受け取った時刻から stamp を引いた値。平均 0.214 秒、最小 0.131 秒、最大 0.518 秒

ロボットの中で同じコマンドを実行した結果はこうでした(2026 年 9 月に測定)。

$ ros2 topic delay /scan
average delay: 0.100
	min: 0.099s max: 0.102s std dev: 0.00124s window: 9
average delay: 0.100
	min: 0.099s max: 0.102s std dev: 0.00122s window: 19
(中略)
average delay: 0.100
	min: 0.098s max: 0.102s std dev: 0.00129s window: 69

ロボットの中で受け取っても、約 0.1 秒の遅れがあります。

測ったもの値
往復にかかる時間(#07 の latency_probe)平均 18.1 ms
ros2 topic delay /scan(ロボットの中)平均 0.100 秒
ros2 topic delay /scan(PC)平均 0.214 秒

ミッション 自分のノードで、障害物の手前で止まる

ここまでは、人がコマンドを打ったりキーを押したりしてロボットを動かしてきました。ミッションでは、ロボットが自分で「前が近いから止まる」と決めるノードを作り、床で動かします。作ったノードが動いたら、自分で考えた動きを 1 つ足します。

このノードには、今日の実験で調べたことがそのまま入ります。

今日の実験ノードのどこに使うか
実験 1・2(QoS)/scan をどの QoS で受け取るか
実験 3・5(座標系と 0° の向き)/scan のどの点を「前」とするか
実験 6(遅れ)どのくらい手前で止まる指令を出すか

進め方は #04 と同じです。自分で決めるのは「何を作るか」と「できたものが決めたとおりか」で、コードは AI に書かせます。次の順に進めます。

  1. 設計シートを書く(M-1)
  2. AI にコードを書かせ、設計シートと突き合わせる(M-2)
  3. パッケージを作り、コードを入れてビルドする(M-3〜M-5)
  4. 机の上で、速度指令を数字で確かめる(M-6)
  5. 先生の合図のあと、床で動かす(M-7)
  6. 自分の動きを 1 つ足し、ためして記録する(M-8・M-9)
決めごと内容
できたと言える条件ロボットが自分で前に進み、箱に一度も当たらずに手前で止まる。箱を手でゆっくり遠ざけると、また進み出す
役割2 人 1 組。1 人がノードを動かし、もう 1 人(見守り役)はロボットの横に立つ。危ないと思ったら「止めて」と声をかける
止め方ノードを動かしている端末で Ctrl+C。それでも止まらなければ、ロボットの電源スイッチを切る

床で歩かせるのは、先生の合図があってからです。M-6 までは、ロボットを机の上に置いたまま進めます。ひとりで学ぶ人は、ほかの人に見守り役を頼みます。

M-1. 設計シートを書く

#04 の「1. 設計シートを書く」の白紙を使います。#04 の counter とちがい、このノードは速度指令を出すので、「止まる条件」の欄を必ず書きます。欄ごとの手がかりは次の表のとおりです。自分で書いてから、下の解答例を開いて見比べます。

欄何を書くか手がかり
このノードの仕事1 文で「前に何もなければゆっくり進み、近づいたら止まる」のように、動きを 1 文で書く
受け取るもの/scan の型・QoS・周期型は実験 2 の ros2 topic info -v /scan の Type の行。QoS は実験 2 の「考える」で考えたもの。周期は #07 で測った ros2 topic hz の値
出すもの速度指令のトピック名・型・周期#08 で teleop_twist_keyboard が送っていた cmd_vel(型は geometry_msgs/msg/Twist)。/scan が届くたびに 1 回出すのではなく、決まった間隔で出し続けます。ロボット側の cmd_vel_guard は、指令が 1 秒届かないと止まるからです(#08)
パラメータあとから変えたくなる数前に進む速さ、止まる距離、「前」の幅、/scan が途絶えたと決めるまでの時間。数をコードに直接書かずにパラメータにしておくと、コードを直さずに起動のときに変えられます
座標系どの点を「前」とするか実験 3 で、laser_frame は base_link の真上でした。実験 5 で、0° が正面かを確かめました。その結果を書きます
止まる条件どんなときに止まるか。すべて書く「前が近い」だけでは足りません。/scan が届かないとき(Wi-Fi が切れたとき)、点が 1 つも測れないとき、Ctrl+C で終わるときにどうするかも書きます
検証の方法机の上で何を打ち、どうなれば正しいかM-6 でそのとおりに確かめます。速度指令は /cmd_vel_test に付け替えて見ます(#08 の実験 3 の前半)
ヒント:止まる距離をいくつにするか(自分の値を決めてから開く)

/scan の距離は、LiDAR の中心からの距離です。LiDAR の中心からロボットの頭の先までは 13 cm ほどあります。stop_distance を 0.35 m にすると、頭の先と箱のすき間は 22 cm ほどで止まる指令が出ます。

指令を出してからも、ロボットは少し進みます。実験 6 の遅れ(0.2 秒ほど)のあいだに 0.2 m/s で進むと 4 cm、そこに Wi-Fi を通る時間と脚が止まるまでの時間(#08)が足されます。それでも 22 cm あれば、箱には当たりません。

設計シートの解答例(自分で書いてから開く)
■ ノード名:obstacle_stop
■ このノードの仕事(1 文):前に障害物がなければゆっくり前に進み、近づいたら止まる
■ 受け取るもの
  トピック名 | 型                        | QoS         | 期待する周期
  /scan      | sensor_msgs/msg/LaserScan | best effort | 5 Hz(#07 で測った値)
■ 出すもの
  トピック名 | 型                      | QoS      | 周期
  cmd_vel    | geometry_msgs/msg/Twist | reliable | 10 Hz(0.1 秒ごと)
■ パラメータ
  名前           | 型    | 初期値 | 単位 | 意味
  forward_speed  | float | 0.2    | m/s  | 前に進む速さ
  stop_distance  | float | 0.35   | m    | LiDAR の中心から、これより近い点が前にあれば止まる
  half_width_deg | float | 30.0   | 度   | 正面から左右に何度までを「前」とするか
  scan_timeout   | float | 1.0    | 秒   | /scan がこれより長く届かなければ止まる
■ 座標系:/scan は laser_frame で測っている。laser_frame は base_link の真上にある(実験 3)。
  0° が正面(実験 5)。角度は 0〜2π で 1 周するので、−π〜π に直してから ±30° と比べる
■ 止まる条件(速度指令を出すノードは必ず書く):
  ・前 ±30° に stop_distance より近い点がある
  ・/scan がまだ 1 回も届いていない、または scan_timeout 秒より長く届いていない
  ・前 ±30° に測れた点が 1 つもない(inf・nan・range_min 未満・range_max 超えは使わない)
  ・Ctrl+C で終わるときは、速度 0 を送ってから終わる
  「止まる」とは、linear.x も angular.z も 0 の Twist を送ること
■ 検証の方法(実行するコマンドと、期待する結果):
  1. 机の上で cmd_vel を /cmd_vel_test に付け替えて起動し、
     ros2 topic echo /cmd_vel_test --field linear.x を見る。
     前に何もなければ 0.2、LiDAR の前に手をかざすと 0.0
  2. ros2 topic info -v /scan で、自分のノードが BEST_EFFORT で受けている
  3. Ctrl+C で止めると、最後に 0.0 が届く
■ 使った AI とプロンプト:Claude(例。自分が使った AI の名前を書く。プロンプトは「M-2. AI に頼む」のもの)

QoS を best effort にしたのは、実験 1 の C のとおり、送り手が reliable でも best effort でも受け取れるからです。出すほうの cmd_vel は、先頭に / を付けずに書いています。机の上では /cmd_vel_test に付け替えて起動するからです。「前に点が 1 つもない」ときに止まるのは、LiDAR に何かあって測れていないのか、本当に何もないのかを、ノードが区別できないからです。分からないときは止まる側に倒します。

M-2. AI に頼む

Windows 側のブラウザで AI を開き、次のプロンプトの最後に自分の設計シートを貼って送ります。

次の設計シートどおりに、ROS 2 Humble で動く Python(rclpy)のノードを書いてください。
パッケージ名は stop_demo、ファイルは stop_demo/obstacle_stop.py です。
・設計シートにないトピックやパラメータは足さないでください。
・Ctrl+C で終わるときに速度 0 を送れるよう、rclpy.init は
  signal_handler_options=SignalHandlerOptions.NO を付けて呼んでください。
・高校生が読むので、大事な行には日本語のコメントを付けてください。
・分からないところがあれば、書く前に私に質問してください。

(ここに自分の設計シートを貼る)

SignalHandlerOptions.NO の行は、#08 の実験 3 の B で考えたことにかかわります。ふつうの書き方だと、Ctrl+C を押した瞬間に ROS 2 との通信が閉じてしまい、終わる前に「止まれ」を送れません。この指定を付けると、Ctrl+C のあとも少しのあいだ送れるので、最後に速度 0 を送ってから終われます。

返ってきたコードは、パッケージに入れる前に設計シートと突き合わせます。見るところは次の 5 つです。説明できない行があったら、AI に「この行は何をしていますか」と聞き返します。

  • create_subscription:型が LaserScan、トピック名が scan、QoS が best effort(qos_profile_sensor_data は best effort の決まった組み合わせ)か
  • create_publisher:型が Twist、トピック名が cmd_vel か
  • create_timer:シートの周期(0.1 秒)で速度指令を出しているか
  • 止まる条件:シートに書いた条件が、それぞれ if の 1 つになっているか。どれにも当てはまらないときだけ前に進むか
  • declare_parameter:名前と初期値がシートと同じか
AI が返したコードの一例(自分で AI に頼んでから開く)
"""前に障害物がなければゆっくり進み、近づいたら止まるノード(#09 のミッションの解答例)。"""
import math
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.signals import SignalHandlerOptions
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan


class ObstacleStop(Node):
    def __init__(self):
        super().__init__('obstacle_stop')
        self.declare_parameter('forward_speed', 0.2)    # 前進の速さ [m/s]
        self.declare_parameter('stop_distance', 0.35)   # LiDAR の中心からこれより近いと止まる [m]
        self.declare_parameter('half_width_deg', 30.0)  # 前方として見る範囲の半分 [度]
        self.declare_parameter('scan_timeout', 1.0)     # /scan がこれだけ届かないと止まる [s]
        self.front = math.inf      # 前方でいちばん近い距離 [m]
        self.last_scan = None      # 最後に /scan を受け取った時刻
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.create_subscription(LaserScan, 'scan', self.on_scan, qos_profile_sensor_data)
        self.create_timer(0.1, self.on_timer)   # 10 Hz で速度指令を出す

    def on_scan(self, msg):
        half = math.radians(self.get_parameter('half_width_deg').value)
        nearest = math.inf
        for i, r in enumerate(msg.ranges):
            # inf・nan と、測れる範囲の外の値は使わない
            if not math.isfinite(r) or r < msg.range_min or r > msg.range_max:
                continue
            a = msg.angle_min + i * msg.angle_increment
            a = math.atan2(math.sin(a), math.cos(a))   # 0〜2π を -π〜π に直す
            if abs(a) <= half and r < nearest:
                nearest = r
        self.front = nearest
        self.last_scan = self.get_clock().now()

    def on_timer(self):
        cmd = Twist()   # 作ったときは速さがすべて 0(止まれ)
        stop_distance = self.get_parameter('stop_distance').value
        timeout = self.get_parameter('scan_timeout').value
        if self.last_scan is None:
            reason = 'まだ /scan が届いていない'
        elif (self.get_clock().now() - self.last_scan).nanoseconds / 1e9 > timeout:
            reason = '/scan が途絶えた'
        elif not math.isfinite(self.front):
            reason = '前方に点が 1 つもない'
        elif self.front < stop_distance:
            reason = '前が近い(%.2f m)' % self.front
        else:
            cmd.linear.x = self.get_parameter('forward_speed').value
            reason = None
        self.pub.publish(cmd)
        if reason:
            self.get_logger().info('止まる:' + reason, throttle_duration_sec=1.0)
        else:
            self.get_logger().info('進む:前は %.2f m' % self.front, throttle_duration_sec=1.0)

    def send_stop(self):
        self.get_logger().info('終わる前に「止まれ」を送ります')
        for _ in range(3):
            self.pub.publish(Twist())
            time.sleep(0.05)


def main(args=None):
    # Ctrl+C のあとも「止まれ」を送れるよう、rclpy に Ctrl+C を処理させない
    rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
    node = ObstacleStop()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.send_stop()
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()

on_scan は /scan が届くたびに呼ばれ、前でいちばん近い距離を self.front にしまうだけです。速度指令を決めて出すのは、0.1 秒ごとに呼ばれる on_timer です。受け取る仕事と出す仕事を分けておくと、/scan が途絶えても on_timer は動き続けるので、「途絶えたから止まる」を判定できます。

AI や聞き方によって書き方は変わります。設計シートどおりなら同じように動きます。

M-3. パッケージを作る

パッケージの名前は stop_demo、ノードの名前は obstacle_stop にします(自分で決めた名前でもかまいません。そのときは、このあとのコマンドの名前も合わせて変えます)。作る場所は #04 と同じ ~/ros2_ws/src です。

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python --node-name obstacle_stop stop_demo --dependencies rclpy sensor_msgs geometry_msgs

#04 のコマンドとちがうのは、--dependencies の後ろです。/scan の型 LaserScan は sensor_msgs、速度指令の型 Twist は geometry_msgs というパッケージに入っているので、この 2 つを書きます。

ros2 pkg create を実行したところ。作られたファイルの名前が順に表示されます。最後の [WARNING] はライセンスを書いていないという知らせで、授業では気にしなくてかまいません
ros2 pkg create を実行したところ。作られたファイルの名前が順に表示されます。最後の [WARNING] はライセンスを書いていないという知らせで、授業では気にしなくてかまいません
find stop_demo -type f | sort
できあがったファイルの一覧。--node-name obstacle_stop を付けたので、ひな形の obstacle_stop.py も入っています
できあがったファイルの一覧。--node-name obstacle_stop を付けたので、ひな形の obstacle_stop.py も入っています

M-4. コードを入れる

ひな形の obstacle_stop.py を開き、中身をすべて AI のコードに置き換えて、Ctrl+S で保存します。コードの貼り方は #04 の「3-3」と同じです(Windows でコピーしたコードを noVNC のクリップボードの入力欄に貼ってから、VSCodium の中で Ctrl+V)。

codium ~/ros2_ws/src/stop_demo/stop_demo/obstacle_stop.py
VSCodium で obstacle_stop.py を AI のコードに置き換えて保存したところ。タブの名前の横に白い丸(●)が残っていたら、まだ保存されていません
VSCodium で obstacle_stop.py を AI のコードに置き換えて保存したところ。タブの名前の横に白い丸(●)が残っていたら、まだ保存されていません

obstacle_stop はパッケージを作ったときに setup.py に登録されているので、今回は書き足さなくてかまいません。登録されているかだけ確かめます。

grep -n -A3 console_scripts ~/ros2_ws/src/stop_demo/setup.py
obstacle_stop = stop_demo.obstacle_stop:main の行があれば登録されています
obstacle_stop = stop_demo.obstacle_stop:main の行があれば登録されています

M-5. ビルドする

#04 と同じく、ワークスペースに移動してビルドし、できたものを今の端末に読み込みます。

cd ~/ros2_ws && colcon build --symlink-install && source install/setup.bash
Summary: … packages finished と出れば、ビルドはできています。stop_demo のほかに、これまでの回で作ったパッケージも一緒にビルドされます
Summary: … packages finished と出れば、ビルドはできています。stop_demo のほかに、これまでの回で作ったパッケージも一緒にビルドされます

Failed や赤い文字が出たら、その少し上に出ているエラーの行をそのまま AI に貼って、何が起きたかを聞きます。Python の書き間違いは、ビルドでは見つからず、次の M-6 で起動したときに初めてエラーになることがあります。

M-6. 机の上で確かめる

ロボットは机の上に置いたまま、robot-status でつながっていることを確かめてから進めます。まだ床では動かさないので、速度指令は #08 と同じく /cmd_vel_test に付け替えて出します。/cmd_vel_test はロボットの脚にはつながっていないので、どんな指令を出してもロボットは動きません。

# 端末 1(ビルドした端末)
ros2 run stop_demo obstacle_stop --ros-args -r cmd_vel:=/cmd_vel_test
# 端末 2
ros2 topic echo /cmd_vel_test --field linear.x

--field linear.x は、Twist のうち前に進む速さ(linear.x)だけを表示する指定です。端末 2 に出る数を見ながら、次の順に確かめ、結果をノートに書きます。打つ前に、それぞれ何が出るかを予想しておきます。

  1. LiDAR の前に何も置かないとき
  2. LiDAR の正面 20 cm ほどに手をかざしたとき
  3. 手を、正面から真横(90°)へゆっくり動かしていったとき。どの向きで数が変わるか
  4. 端末 1 で Ctrl+C を押したとき。端末 2 に最後に出た数は何か
結果を見る(予想を書き、自分で実行してから開く)
左が自分のノード、右が /cmd_vel_test。前が空いているので「進む」と出て、0.2 が並んでいます
左が自分のノード、右が /cmd_vel_test。前が空いているので「進む」と出て、0.2 が並んでいます
前が近いと「止まる」と出て、0.0 が並びます。この画面は記録データ(#10 で使う room_scan)で撮ったので、手をかざす代わりに -p stop_distance:=0.45 を付けて起動しました
前が近いと「止まる」と出て、0.0 が並びます。この画面は記録データ(#10 で使う room_scan)で撮ったので、手をかざす代わりに -p stop_distance:=0.45 を付けて起動しました

3 つめは、正面から 30° を超えたあたりで 0.0 から 0.2 に戻れば、設計シートの half_width_deg のとおりです。4 つめで最後に 0.0 が出ていれば、Ctrl+C のあとに「止まれ」を送れています。端末 1 には「終わる前に「止まれ」を送ります」と出ます。

パラメータは、コードを直さずに起動のときに変えられます。たとえば、止まる距離を 0.5 m にして起動すると、手をかざしたときにより遠くで 0.0 になります。

ros2 run stop_demo obstacle_stop --ros-args -r cmd_vel:=/cmd_vel_test -p stop_distance:=0.5

最後に、自分のノードが /scan を設計シートどおりの QoS で受けているかを見ます。ノードを動かしたまま、新しい端末で打ちます。

ros2 topic info -v /scan
受ける側(Subscription)に obstacle_stop が出ていて、Reliability が BEST_EFFORT
受ける側(Subscription)に obstacle_stop が出ていて、Reliability が BEST_EFFORT

ここまで設計シートの「検証の方法」のとおりになったら、端末 1 を Ctrl+C で止めます。

M-7. 床で動かす(先生の合図のあと)

  1. 見守り役が、ロボットを囲いの中の床に置き、正面 1 m ほど先に箱を置く(実験 5 で使った背の高い箱)。
  2. 端末 2 の echo を Ctrl+C で止め、ros2 topic echo /cmd_vel --field linear.x を打ち直す(今度は本物の /cmd_vel を見る)。
  3. 端末 1 で、付け替えをせずにノードを起動する。ロボットが歩き出す。
  4. 箱の手前で止まったら、見守り役が箱を手でゆっくり遠ざける。ロボットがどう動くかを見る。
  5. 箱を横から差し出したり、低い箱に替えたりして、止まるときと止まらないときを探す。
  6. 終わったら端末 1 で Ctrl+C を押す。
# 端末 1(先生の合図のあと)
ros2 run stop_demo obstacle_stop

付け替え(-r cmd_vel:=/cmd_vel_test)を外すと、ノードは /cmd_vel に出し、ロボットの脚が動きます。M-6 の数が、そのままロボットの動きになります。

試したことロボットはどう動いたか設計シートのどの行がはたらいたか
正面の箱に近づく
箱を遠ざける
自分で考えた置き方
止まらない・止まりすぎるときの手がかり(試してから開く)
  • 低い箱で止まらない:LiDAR は、自分の高さの面を水平に見ています(実験 3 で、base_link の 18 cm 上にありました)。それより低いものは /scan に映りません。ノードの書き方ではなく、センサーの見え方の限界です。
  • 斜めから近づく箱で止まらない:「前」を ±30° にしたので、それより外にある箱は見ていません。-p half_width_deg:=45.0 のように広げて試せます。広げすぎると、横の壁で止まるようになります。
  • 起動してもまったく歩かない:端末 1 の「止まる:」のあとの理由を読みます。「まだ /scan が届いていない」なら robot-status、「前が近い」なら正面に何があるかを見ます。
  • 箱に近づきすぎる:-p stop_distance:=0.45 のように、止まる距離を大きくして起動し直します。

M-8. 自分の動きを 1 つ足す

止まれるようになったら、自分で考えた動きを 1 つ足します。次の表から選んでも、自分で考えてもかまいません。後ろに下がる動きは作らないでください。このノードは前しか見ていないので、後ろに何があっても下がってしまいます。

足す動き設計シートで変えるところ
A近づくほど遅くなり、なめらかに止まるパラメータ slow_distance(遅くし始める距離)を足す。進むときの速さを、前の距離から決める
B止まったら、その場で向きを変え、前が空いたらまた進むパラメータ turn_speed を足す。「前が近い」ときは止まらずに angular.z を出す
C自分で考えた動きどこを変えるかを自分で書く。止まる条件のうち、「/scan が届かない」「点が 1 つもない」「Ctrl+C」の 3 つは残す

もとの obstacle_stop.py は残しておき、写したファイルを書きかえます。ここでは B を選んだとして、ファイル名を obstacle_turn.py にします。

  1. 設計シートを写し、ノード名と変えたところを書き直す。
  2. もとのファイルを写す:cp は「ファイルを写す」コマンドで、左のファイルを右の名前で写します。
  3. AI に、下のプロンプトで変えたところだけを入れてもらう。
  4. obstacle_turn.py を開いて中身を置き換え、Ctrl+S で保存する。
  5. setup.py に obstacle_turn を登録する(#04 の「3-4」と同じ)。
  6. ビルドし直してから、M-6 と同じように机の上で確かめる。
  7. 先生の合図のあと、M-7 と同じように床で動かす。
cd ~/ros2_ws/src/stop_demo/stop_demo
cp obstacle_stop.py obstacle_turn.py
codium obstacle_turn.py
前に作ったノード obstacle_stop.py(下に貼る)に、動きを 1 つ足したいです。
新しいファイルは stop_demo/obstacle_turn.py、ノード名は obstacle_turn にしてください。
変えたいところは、下の設計シートの「変えたところ」の行だけです。
ほかのところ(止まる条件、/scan の受け取り方、Ctrl+C の扱い)は変えないでください。
変えた行には「# 変えた」とコメントを付けてください。

(ここに obstacle_stop.py の中身を貼る)

(ここに書き直した設計シートを貼る。変えた行の頭に「変えたところ:」と書く)

返ってきたコードは、「# 変えた」の行がシートの「変えたところ」と合っているかを見ます。とくに、super().__init__('obstacle_turn') のノード名が変わっているかを確かめます。名前が obstacle_stop のままだと、2 つのノードが同じ名前になり、ros2 node info でどちらを見ているのか分からなくなります。

setup.py の console_scripts に 1 行足して保存します(上の行の最後のカンマを忘れないでください)。

codium ~/ros2_ws/src/stop_demo/setup.py
    entry_points={
        'console_scripts': [
            'obstacle_stop = stop_demo.obstacle_stop:main',
            'obstacle_turn = stop_demo.obstacle_turn:main'
        ],
    },

setup.py を変えたので、必ずビルドし直します。ビルドしないと、起動したときに「No executable found」と出ます(#04 の「うまくいかないとき」)。

cd ~/ros2_ws && colcon build --symlink-install && source install/setup.bash
上:setup.py に 2 つのノードが登録されたところ。下:ビルドし直したところ
上:setup.py に 2 つのノードが登録されたところ。下:ビルドし直したところ

机の上で確かめるときは、B なら angular.z(向きを変える速さ)も見ます。端末をもう 1 つ開いて、2 つ並べて見ます。

# 端末 1
ros2 run stop_demo obstacle_turn --ros-args -r cmd_vel:=/cmd_vel_test
# 端末 2
ros2 topic echo /cmd_vel_test --field linear.x
# 端末 3
ros2 topic echo /cmd_vel_test --field angular.z
結果を見る(予想を書き、自分で実行してから開く)
B のノードを机の上で動かしたところ。前が近いので、前に進む速さは 0.0、向きを変える速さは 0.6 になっています。この画面も記録データで撮ったので、-p stop_distance:=0.45 を付けて起動しました
B のノードを机の上で動かしたところ。前が近いので、前に進む速さは 0.0、向きを変える速さは 0.6 になっています。この画面も記録データで撮ったので、-p stop_distance:=0.45 を付けて起動しました

手をかざすと、端末 2 は 0.0、端末 3 は 0.6 になります。手をどけると、端末 2 は 0.2、端末 3 は 0.0 に戻ります。

書きかえて保存したあと、ビルドし直さずに起動し直すだけで反映されるのは、--symlink-install を付けてビルドした Python のファイルを直したときだけです。setup.py を変えたとき、新しいファイルを足したときはビルドし直します。迷ったらビルドし直してかまいません。

A と B のコードの一例(自分で作ってから開く)

A:obstacle_slow.py

"""近づくほど遅くなり、近づいたら止まるノード(#09 の「動きを 1 つ足す」A の解答例)。"""
import math
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.signals import SignalHandlerOptions
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan


class ObstacleSlow(Node):
    def __init__(self):
        super().__init__('obstacle_slow')
        self.declare_parameter('forward_speed', 0.2)    # 前進の速さ [m/s]
        self.declare_parameter('stop_distance', 0.35)   # LiDAR の中心からこれより近いと止まる [m]
        self.declare_parameter('slow_distance', 0.8)    # これより近いと遅くし始める [m]
        self.declare_parameter('half_width_deg', 30.0)  # 前方として見る範囲の半分 [度]
        self.declare_parameter('scan_timeout', 1.0)     # /scan がこれだけ届かないと止まる [s]
        self.front = math.inf      # 前方でいちばん近い距離 [m]
        self.last_scan = None      # 最後に /scan を受け取った時刻
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.create_subscription(LaserScan, 'scan', self.on_scan, qos_profile_sensor_data)
        self.create_timer(0.1, self.on_timer)   # 10 Hz で速度指令を出す

    def on_scan(self, msg):
        half = math.radians(self.get_parameter('half_width_deg').value)
        nearest = math.inf
        for i, r in enumerate(msg.ranges):
            # inf・nan と、測れる範囲の外の値は使わない
            if not math.isfinite(r) or r < msg.range_min or r > msg.range_max:
                continue
            a = msg.angle_min + i * msg.angle_increment
            a = math.atan2(math.sin(a), math.cos(a))   # 0〜2π を -π〜π に直す
            if abs(a) <= half and r < nearest:
                nearest = r
        self.front = nearest
        self.last_scan = self.get_clock().now()

    def on_timer(self):
        cmd = Twist()   # 作ったときは速さがすべて 0(止まれ)
        stop_distance = self.get_parameter('stop_distance').value
        timeout = self.get_parameter('scan_timeout').value
        if self.last_scan is None:
            reason = 'まだ /scan が届いていない'
        elif (self.get_clock().now() - self.last_scan).nanoseconds / 1e9 > timeout:
            reason = '/scan が途絶えた'
        elif not math.isfinite(self.front):
            reason = '前方に点が 1 つもない'
        elif self.front < stop_distance:
            reason = '前が近い(%.2f m)' % self.front
        else:
            speed = self.get_parameter('forward_speed').value
            slow_distance = self.get_parameter('slow_distance').value
            if self.front < slow_distance:
                # stop_distance で 0、slow_distance で forward_speed になるよう、まっすぐ比例させる
                speed *= (self.front - stop_distance) / (slow_distance - stop_distance)
            cmd.linear.x = speed
            reason = None
        self.pub.publish(cmd)
        if reason:
            self.get_logger().info('止まる:' + reason, throttle_duration_sec=1.0)
        else:
            self.get_logger().info('進む:前は %.2f m、速さ %.2f' % (self.front, cmd.linear.x), throttle_duration_sec=1.0)

    def send_stop(self):
        self.get_logger().info('終わる前に「止まれ」を送ります')
        for _ in range(3):
            self.pub.publish(Twist())
            time.sleep(0.05)


def main(args=None):
    # Ctrl+C のあとも「止まれ」を送れるよう、rclpy に Ctrl+C を処理させない
    rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
    node = ObstacleSlow()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.send_stop()
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()

速さは「前の距離が stop_distance のとき 0、slow_distance のとき forward_speed」になるよう、そのあいだをまっすぐ比例させています。ノード名とファイル名は obstacle_slow で、setup.py には 'obstacle_slow = stop_demo.obstacle_slow:main' を足します。

B:obstacle_turn.py

"""近づいたら止まり、その場で左へ向きを変えて、前が空いたらまた進むノード(#09 の「動きを 1 つ足す」B の解答例)。"""
import math
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.signals import SignalHandlerOptions
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan


class ObstacleTurn(Node):
    def __init__(self):
        super().__init__('obstacle_turn')
        self.declare_parameter('forward_speed', 0.2)    # 前進の速さ [m/s]
        self.declare_parameter('stop_distance', 0.35)   # LiDAR の中心からこれより近いと止まる [m]
        self.declare_parameter('turn_speed', 0.6)       # 向きを変える速さ [rad/s]。+ で左回り
        self.declare_parameter('half_width_deg', 30.0)  # 前方として見る範囲の半分 [度]
        self.declare_parameter('scan_timeout', 1.0)     # /scan がこれだけ届かないと止まる [s]
        self.front = math.inf      # 前方でいちばん近い距離 [m]
        self.last_scan = None      # 最後に /scan を受け取った時刻
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.create_subscription(LaserScan, 'scan', self.on_scan, qos_profile_sensor_data)
        self.create_timer(0.1, self.on_timer)   # 10 Hz で速度指令を出す

    def on_scan(self, msg):
        half = math.radians(self.get_parameter('half_width_deg').value)
        nearest = math.inf
        for i, r in enumerate(msg.ranges):
            # inf・nan と、測れる範囲の外の値は使わない
            if not math.isfinite(r) or r < msg.range_min or r > msg.range_max:
                continue
            a = msg.angle_min + i * msg.angle_increment
            a = math.atan2(math.sin(a), math.cos(a))   # 0〜2π を -π〜π に直す
            if abs(a) <= half and r < nearest:
                nearest = r
        self.front = nearest
        self.last_scan = self.get_clock().now()

    def on_timer(self):
        cmd = Twist()   # 作ったときは速さがすべて 0(止まれ)
        stop_distance = self.get_parameter('stop_distance').value
        timeout = self.get_parameter('scan_timeout').value
        if self.last_scan is None:
            reason = 'まだ /scan が届いていない'
        elif (self.get_clock().now() - self.last_scan).nanoseconds / 1e9 > timeout:
            reason = '/scan が途絶えた'
        elif not math.isfinite(self.front):
            reason = '前方に点が 1 つもない'
        elif self.front < stop_distance:
            # 前には進まず、その場で向きを変える
            cmd.angular.z = self.get_parameter('turn_speed').value
            self.get_logger().info('向きを変える:前が近い(%.2f m)' % self.front, throttle_duration_sec=1.0)
            reason = None
        else:
            cmd.linear.x = self.get_parameter('forward_speed').value
            reason = None
        self.pub.publish(cmd)
        if reason:
            self.get_logger().info('止まる:' + reason, throttle_duration_sec=1.0)
        elif cmd.linear.x > 0.0:
            self.get_logger().info('進む:前は %.2f m' % self.front, throttle_duration_sec=1.0)

    def send_stop(self):
        self.get_logger().info('終わる前に「止まれ」を送ります')
        for _ in range(3):
            self.pub.publish(Twist())
            time.sleep(0.05)


def main(args=None):
    # Ctrl+C のあとも「止まれ」を送れるよう、rclpy に Ctrl+C を処理させない
    rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
    node = ObstacleTurn()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.send_stop()
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()

向きを変えるのは左回り(angular.z が+)だけです。右に箱があっても左に回ります。左右どちらが空いているかを見て回る向きを決めるのは、#11 で読む scan_avoid がしていることです。

M-9. 自分でいじわるな置き方を考えて、ためす

床で動いたら、自分のノードが失敗しそうな箱の置き方を考えます。設計シートの「前」の範囲(±30°)や止まる距離を見ながら、「ここに置いたら止まれないはず」という置き方を 1 つ決め、予想を書いてから、先生の合図のあとにためします。

たとえば、箱を正面から少し横にずらして置く、細い物(ペットボトルなど)を置く、といった置き方が考えられます。予想どおり失敗したら、設計シートのどの行が原因かを書きます。予想が外れたら、なぜ外れたかを考えます。

記録すること(ノートに)書くこと
自分が足した動き
考えた置き方と、ためす前の予想
結果と、設計シートのどの行が関わっていたか

ミッションの記録は、レポートの決まった欄はありません。本質の問いの 2 つめ(遅れがどこで生まれるか)を考えるときに、M-1 のヒント(止まる距離の決め方)と合わせて使ってください。#11 では、このノードを育てて、箱を置いた囲いの中を 1 分間ぶつからずに歩かせます。stop_demo のパッケージは消さずに残しておきます。

片付け:ノードを Ctrl+C で止め、ロボットを机の上に戻します。次の回もつないで使うので、つないだままでかまいません。

本質の問い

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

  • /scan の 1 つの点について「その点はロボットのどこにあるか」を答えるには、どんな情報が必要か。3 つ挙げ、それぞれ今日どのコマンドで見たかも書く。
  • PC で測った ros2 topic delay の平均(0.2 秒ほど)は、#07 の往復時間(20 ms ほど)よりずっと大きい。どこで差が生まれていそうか。考えられることを 2 つ以上書く。

1 つめの手がかりは、実験 3(座標系)と実験 6(header)です。2 つめは、ロボットの中で測った delay(約 0.1 秒)と PC で測った delay の差がどこから来たかを考えます。

考え方の例(答えを書いてから開く)
/scan の 1 点の場所を決める 3 つの情報
/scan の 1 点の場所を決める 3 つの情報

1 つめ:① その点の距離と角度(ranges の値と、何番目の点か)、② laser_frame と base_link の関係(TF。この LiDAR は 18 cm 上)、③ いつ測ったか(stamp)。ロボットが歩いていると、同じ距離と角度でも、測った時刻によって部屋の中での場所が変わります。

  • ロボットの中ですでに約 0.1 秒ある。LiDAR が 1 回転するのにかかる時間(scan_time、この LiDAR では約 0.1 秒)と近いので、1 回転を測り始めた時刻を stamp にして、測り終えてから送っていると考えると合う。
  • 残りの 0.1 秒ほどは、ブリッジと Wi-Fi の区間。ロボット側のブリッジは /scan を 10 Hz から 5 Hz に間引いていて、どの回を通すかでも遅れが変わりそう。
  • PC とロボットの時計がずれていると、その分だけ delay が増えたり減ったりする。往復時間は PC の時計だけで測るので、時計のずれの影響を受けない。

つまずきやすい点

症状見るところ
ros2 topic echo に何も出ず、黄色の警告だけが出る警告の最後の「Last incompatible policy」。QoS の組み合わせ(実験 1 の A)
view_frames の PDF に base_link しか出ないrobot-status でブリッジが動作中か。つないでから数秒待ってから実行する
tf2_echo が「frame does not exist」と出し続ける座標系の名前のつづり。最初の数回はまだ TF が集まっていないだけのこともある
RViz に点が出ないFixed Frame のつづりと、LaserScan の Topic。Status の行を開くとエラーの理由が書いてある
ros2 topic delay がマイナスや数秒になるPC とロボットの時計がずれている。授業では自分で直そうとせず、先生に伝える。ひとりで学ぶ人は、PC とロボットの両方で date を実行して時刻を比べる

まとめ

確かめたことわかったこと
QoS の組み合わせ(実験 1)名前と型が合っていても、受ける側が送る側より多くを求めると届かない
実物の /scan(実験 2)この LiDAR は RELIABLE で送っている。受ける側を best_effort にしておけば、送り手の QoS によらず受け取れる
座標系(実験 3・4・5)/scan は laser_frame で測っていて、laser_frame は base_link の 18 cm 上。0° が正面かは実物で確かめる
時刻(実験 6)データには測った時刻が付いている。受け取るまでの遅れは、ロボットの中の分とその先の分に分けて考える
ミッション設計シートに書いた止まる条件が、そのままロボットの動きになる。どの QoS で受けるか、どの点を「前」とするか、どれだけ手前で止めるかも、今日の実験の結果からシートに書いて決める

次の回は、今日作ったノードを床で走らせながら /scan を rosbag で記録します。そしてロボットにつながずに記録を再生し、自分のノードを直してから、もう一度床で動かします。

トップへ戻る