Hello, ROS 2：パブリッシャ・サブスクライバ・tf2

ROS 2は、ロボットソフトウェアのためのオープンソースのミドルウェアです。ドライバ、推定器、プランナ、コントローラは別々のプログラムとして動作し、ROS 2を介してデータをやり取りします。このレッスンは、Pythonとレッスン2の座標系を理解していて、ROS 2を初めて実際に動かしてみたい読者向けです。まずパブリッシャとサブスクライバを書きます。続いてレッスン7のodomからbase_link、laserへと続く座標系ツリーをtf2で構築し、それを使ってセンサの読み取り値を座標系間で移します。どちらのプログラムも1つのプロセス内で動作して自動で終了するため、デスクトップ環境のないコンテナでも実行できます。

ROS 2 Jazzy Jalisco

本レッスンのコードはすべて、ROS 2 Jazzy Jalisco（LTS、2024年5月）でテストしています。これはROS 2の10番目のリリースです。ROS 2のドキュメントによれば、2024年5月23日にリリースされ、2029年5月までサポートされます。Tier 1プラットフォームはUbuntu 24.04（amd64とarm64）とWindows 10です。JazzyではmacOSはTier 3プラットフォームで、対象はamd64のみです。そのため、MacではApple SiliconでもIntelでも、Linuxコンテナを使うのが現実的です。

Docker Hubの公式rosイメージは、amd64とarm64の両方に向けてjazzy-ros-baseタグを公開しています。コアライブラリ、rclpy、tf2、標準メッセージを含みますが、グラフィカルなツールは含みません。macOSではDockerランタイムをインストールし、以下の2つのPythonブロックをフォルダ内にファイルとして保存してください。その後、そのフォルダをマウントしてコンテナ内でシェルを起動します。

# On macOS (or Linux) with Docker installed. The first pull needs a network connection.
docker pull ros:jazzy-ros-base
mkdir -p ~/ros2-hello && cd ~/ros2-hello    # save talker_listener.py and tf2_demo.py here
docker run -it --rm --name ros2-hello -v "$PWD":/work -w /work ros:jazzy-ros-base bash

イメージのエントリポイントが/opt/ros/jazzy/setup.bashを読み込むため、プロンプトですぐにpython3 talker_listener.pyを実行できます。後でコマンドラインツールを使う際には、同じコンテナで2つ目のシェルが必要です。そのためには、Mac側で次を実行します。

docker exec -it ros2-hello bash -c "source /opt/ros/jazzy/setup.bash && exec bash"

ノード・トピック・メッセージ・QoS

ノード はROSグラフの参加者の1つで、通常は1つの役割を持つ1つのプログラムです。LiDARドライバ、オドメトリ推定器、経路プランナなどがこれに当たります。トピック は、/scanや/cmd_velのような名前付きのチャネルです。ノードは相手が誰かを知らないまま、トピックにメッセージをパブリッシュし、トピックをサブスクライブします。メッセージは、インタフェースファイルで定義された型付きのデータ構造です。geometry_msgs/msg/Twistは速度指令を、sensor_msgs/msg/LaserScanはLiDARのスキャンを運びます。トピックはデータのストリームに向いています。ROS 2には、要求と応答のためのサービスと、フィードバックを伴う長時間のゴールのためのアクションもあります。

内部では、ROS 2はミドルウェア層を介してDDSの実装にメッセージを渡します。Jazzyのデフォルトはe​Prosima Fast DDSです。デフォルトでは、ノードはローカルネットワーク上で自身を通知し合うことで互いを自動的に発見し、中央サーバは必要ありません。

各パブリッシャとサブスクリプションは、QoS（quality of service、サービス品質）プロファイルを持ちます。デフォルトは、直近10件のメッセージを保持（keep last 10）、信頼性はreliable、耐久性はvolatileです。volatileとは、後から参加したノードが過去のメッセージを受け取らないという意味です。センサドライバは、代わりにbest-effortの信頼性でパブリッシュすることがよくあります。確実に届く古いスキャンよりも、新しいスキャンのほうが重要だからです。サブスクリプションがパブリッシャと接続するのは、サブスクリプションが要求する品質以上をパブリッシャが提供する場合に限られます。reliableのサブスクリプションは、best-effortのパブリッシャからは何も受け取りません。rclpyはそのような組を発見すると「incompatible QoS」の警告をログに出します。しかしコールバックには何も届かず、この警告は流れの速いログの中では見落としがちです。トピックは存在するのにコールバックが一向に呼ばれないときは、まずQoSの不一致を確認してください。

1つのプロセスで動くtalkerとlistener

実際のロボットでは、パブリッシャとサブスクライバは別々のプログラムで、ローンチファイルから起動します。最初のテストとしては、1つのプロセスのほうが簡単で、2つ目のターミナルも要りません。以下の各クラスが1つのノードです。talkerはタイマのコールバックから毎秒10回、文字列をパブリッシュします。listenerは受信したメッセージをすべて保存します。

ノードは、何かがそのコールバックを呼び出すまで何もしません。それを担うのがエグゼキュータです。rclpy.spin(node)は永久に動き続けるため、代わりにSingleThreadedExecutorを作成して両方のノードを追加し、短いタイムアウトを付けたspin_onceをループで呼び出します。各呼び出しは、1件の処理（タイマの発火またはメッセージの到着）を最長0.05 s待ち、そのコールバックを実行します。ループは、メッセージを5件受信するか200回呼び出すかのいずれか早いほうで止まります。その後ノードを破棄し、rclpy.shutdown()でミドルウェアを解放します。talker_listener.pyとして保存してください。

# ros2
import rclpy
from rclpy.executors import SingleThreadedExecutor
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from std_msgs.msg import String


class Talker(Node):
    def __init__(self):
        super().__init__("talker")
        qos = QoSProfile(history=HistoryPolicy.KEEP_LAST, depth=10, reliability=ReliabilityPolicy.RELIABLE)
        self.pub = self.create_publisher(String, "chatter", qos)
        self.count = 0
        self.timer = self.create_timer(0.1, self.tick)       # 10 Hz

    def tick(self):
        msg = String()
        msg.data = f"hello {self.count}"
        self.pub.publish(msg)
        self.count += 1


class Listener(Node):
    def __init__(self):
        super().__init__("listener")
        self.received = []
        self.sub = self.create_subscription(String, "chatter", self.on_msg, 10)

    def on_msg(self, msg: String):
        self.received.append(msg.data)


rclpy.init()
talker, listener = Talker(), Listener()
executor = SingleThreadedExecutor()
executor.add_node(talker)
executor.add_node(listener)

for _ in range(200):                      # bounded: at most about 200 x 0.05 s
    executor.spin_once(timeout_sec=0.05)
    if len(listener.received) >= 5:
        break

print(f"talker published {talker.count} messages")
print(f"listener received {len(listener.received)}: {listener.received}")
print("topics seen by the listener node:", sorted(n for n, _ in listener.get_topic_names_and_types()))

executor.shutdown()
talker.destroy_node()
listener.destroy_node()
rclpy.shutdown()
print("shut down cleanly")

talker published 5 messages
listener received 5: ['hello 0', 'hello 1', 'hello 2', 'hello 3', 'hello 4']
topics seen by the listener node: ['/chatter', '/parameter_events', '/rosout']
shut down cleanly

5件のメッセージがすべて、順番どおりに届きました。作成していないトピックが2つ表示されています。/rosoutはすべてのノードのログメッセージを運び、/parameter_eventsはパラメータの変更を通知します。ネットワークが混雑している場合や、2つの別プロセス間で通信する場合は、発見が完了する前に最初の1、2件のメッセージが失われることがあります。デフォルトの耐久性がvolatileだからです。出力がhello 1から始まっていたら、それが理由です。

std_msgs/msg/Stringは昔から最初に使われるメッセージですが、現在はそのインタフェースファイル自体に非推奨と記されており、意味のある名前のメッセージを定義するよう勧めています。テストに使う分には問題ありません。実際のインタフェースでは、温度の読み取り値ならsensor_msgs/msg/Temperatureのように具体的な型を使います。そうすれば、すべてのサブスクライバが単位と座標系を把握できます。

同じグラフをコマンドラインで見る

ros2コマンドラインツールで、動作中のシステムを調べられます。コンテナ内の1つ目のシェルで、コマンドラインから2 Hzでパブリッシュします。2つ目のシェル（上記のdocker execコマンドで開いたもの）で、それを調べます。

# shell 1: publish a String on /chatter twice a second until Ctrl-C
ros2 topic pub -r 2 /chatter std_msgs/msg/String "{data: hello from the CLI}"

# shell 2: look at the graph
ros2 topic list                      # /chatter, /parameter_events, /rosout
ros2 topic echo --once /chatter      # print one message and exit
ros2 topic info -v /chatter          # publishers, subscribers and their QoS
ros2 topic hz /chatter               # measured publishing rate
ros2 interface show std_msgs/msg/String
ros2 node list

ros2 topic info -vはすべてのエンドポイントのQoSを表示します。そのため、前述の気づきにくい不一致を調べるときに使うツールです。その他のコマンドはros2 CLIチートシートにまとめています。

tf2：動く座標系

ロボットには多くの座標系があります。オドメトリ座標系、本体、各センサ、アームの各関節などです。センサの読み取り値はすべて、それを取得したセンサの座標系で表されます。LiDARの点をナビゲーションに使うには、それをodomまたはmapで表す必要があります。そのためには複数の変換をつなげる必要があり、その中には固定のものも、時間とともに変化するものもあります。tf2は、この管理を行うROS 2のライブラリです。

座標系は木構造をなし、各座標系の親はちょうど1つです。各変換はgeometry_msgs/msg/TransformStampedとしてパブリッシュされます。これはタイムスタンプ、親のframe_id、child_frame_id、メートル単位の並進、四元数で表した回転を持ちます。ROSのメッセージでは四元数を（x, y, z, w）の順に並べ、wが最後です。変化する変換は/tfトピックで送られ、タイムスタンプとともにバッファされます。センサの取り付け位置のように変化しない変換は、/tf_staticで1回だけ送られます。Jazzyの静的ブロードキャスタはtransient-localの耐久性と深さ1でパブリッシュするため、後から起動したノードもそれらを受け取れます。

変換を参照するために、ノードはBufferとTransformListenerを作成します。Bufferは最近の変換を保存し、TransformListenerは両方のトピックをサブスクライブしてバッファを満たします。その上でlookup_transform(target, source, time)を呼ぶと、データをsource座標系からtarget座標系へ移す変換が返ります。その際、ツリーの辺を必要なだけつなげます。時刻ゼロであるTime()を指定すると、「利用可能な最新のもの」という意味になります。

静的変換と動的変換を1つのプロセスで

2番目のブロックでは、レッスン7のREP 105の連鎖を再利用します。ブロードキャスタノードは、base_linkからlaserへの変換を静的変換として1回パブリッシュします。レーザは車軸の0.10 m前方、0.20 m上方にあります。また、ロボットが0.2 rad/sで旋回しながら0.5 m/sで走行するのに合わせて、odomからbase_linkへの変換を50 Hzでパブリッシュします。出力を再現可能にするため、オドメトリの時刻はティックのカウンタから求めます。ブロードキャスタは50ティック、つまり1 sの走行後に停止します。2つ目のノードがバッファとリスナを保持します。その後、odomからlaserへの変換を参照し、センサの2 m前方にあるLiDARの反射点をodom座標系に移します。tf2_demo.pyとして保存してください。

# ros2
import math

import rclpy
from geometry_msgs.msg import PointStamped, TransformStamped
from rclpy.duration import Duration
from rclpy.executors import SingleThreadedExecutor
from rclpy.node import Node
from rclpy.time import Time
from tf2_ros import Buffer, StaticTransformBroadcaster, TransformBroadcaster, TransformListener
import tf2_geometry_msgs  # noqa: F401  registers PointStamped with Buffer.transform


def make_tf(stamp, parent: str, child: str, x: float, y: float, z: float, yaw: float) -> TransformStamped:
    t = TransformStamped()
    t.header.stamp = stamp
    t.header.frame_id = parent
    t.child_frame_id = child
    t.transform.translation.x, t.transform.translation.y, t.transform.translation.z = x, y, z
    # Rotation about z only. ROS quaternions are (x, y, z, w).
    t.transform.rotation.z = math.sin(yaw / 2.0)
    t.transform.rotation.w = math.cos(yaw / 2.0)
    return t


class Broadcaster(Node):
    """Publishes odom -> base_link as the robot drives, and base_link -> laser once (static)."""

    def __init__(self):
        super().__init__("frame_broadcaster")
        self.static_br = StaticTransformBroadcaster(self)
        self.dynamic_br = TransformBroadcaster(self)
        now = self.get_clock().now().to_msg()
        self.static_br.sendTransform(make_tf(now, "base_link", "laser", 0.10, 0.0, 0.20, 0.0))
        self.ticks = 0
        self.timer = self.create_timer(0.02, self.tick)    # 50 Hz

    def tick(self):
        # Pretend odometry: drive forward at 0.5 m/s while turning at 0.2 rad/s.
        # Time comes from the tick count so the printed numbers are repeatable.
        t = self.ticks * 0.02
        self.ticks += 1
        if self.ticks == 50:
            self.timer.cancel()                             # stop after 1 s of driving
        v, w = 0.5, 0.2
        yaw = w * t
        x = v / w * math.sin(yaw)
        y = v / w * (1.0 - math.cos(yaw))
        self.dynamic_br.sendTransform(make_tf(self.get_clock().now().to_msg(), "odom", "base_link", x, y, 0.0, yaw))


rclpy.init()
broadcaster = Broadcaster()
listener_node = Node("frame_listener")
tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, listener_node, spin_thread=False)

executor = SingleThreadedExecutor()
executor.add_node(broadcaster)
executor.add_node(listener_node)

for _ in range(200):                                  # bounded: about 4 s at most
    executor.spin_once(timeout_sec=0.02)
    if broadcaster.ticks >= 50:
        break
for _ in range(10):                                   # let the listener drain the last messages
    executor.spin_once(timeout_sec=0.02)

tf = tf_buffer.lookup_transform("odom", "laser", Time(), timeout=Duration(seconds=0.0))
tr, q = tf.transform.translation, tf.transform.rotation
yaw = 2.0 * math.atan2(q.z, q.w)
print(f"odom -> laser: x={tr.x:.3f} y={tr.y:.3f} z={tr.z:.3f} m, yaw={yaw:.3f} rad")

# A laser return 2 m straight ahead of the sensor, expressed in the odom frame.
p = PointStamped()
p.header.frame_id = "laser"
p.header.stamp = tf.header.stamp
p.point.x = 2.0
p_odom = tf_buffer.transform(p, "odom")
print(f"point 2 m ahead of the laser, in odom: x={p_odom.point.x:.3f} y={p_odom.point.y:.3f} z={p_odom.point.z:.3f}")
print(tf_buffer.all_frames_as_string().strip())

executor.shutdown()
broadcaster.destroy_node()
listener_node.destroy_node()
rclpy.shutdown()
print("shut down cleanly")

odom -> laser: x=0.585 y=0.067 z=0.200 m, yaw=0.196 rad
point 2 m ahead of the laser, in odom: x=2.547 y=0.457 z=0.200
Frame laser exists with parent base_link.
Frame base_link exists with parent odom.
shut down cleanly

レッスン7の円弧の式を使って、結果を手計算で確かめます。最後のティックでは t = 0.98 s で、方位は0.2 × 0.98、つまり0.196 radです。ベースはx = 2.5 sin(0.196) = 0.487 m、y = 2.5 (1 - cos(0.196)) = 0.048 mの位置にあります。ここで2.5 mは旋回半径v / omegaです。レーザは方位方向にさらに0.10 m先にあり、x = 0.487 + 0.10 cos(0.196) = 0.585、y = 0.048 + 0.10 sin(0.196) = 0.067です。高さは静的変換だけから決まり、0.200 mです。レーザの2 m前方の点は、同じ方位でベースの2.10 m前方にあたるため、x = 2.547、y = 0.457となります。表示された数値と一致します。

tf2_geometry_msgsは、副作用のためだけにインポートしています。このインポートにより、Buffer.transformがPointStampedなどのgeometryメッセージ型を扱えるようになります。インポートしないと、Buffer.transformは型が読み込まれていないかサポートされていないというTypeExceptionを送出します。このエラーには、不足しているモジュール名が示されません。

コマンドラインで変換を扱う

同じツリーは、tf2_rosに含まれるツールでも構築・確認できます。

# shell 1: publish the static laser mount
ros2 run tf2_ros static_transform_publisher --x 0.1 --z 0.2 --frame-id base_link --child-frame-id laser

# shell 2: print the transform from laser data into base_link once a second
ros2 run tf2_ros tf2_echo base_link laser
ros2 topic echo --once /tf_static

static_transform_publisherは、メートル単位の並進に加えて、ラジアン単位のロール・ピッチ・ヨー、または--qx --qy --qz --qwによる四元数を受け取ります。ros-baseイメージに含まれるros2 run tf2_tools view_framesは、数秒間待ち受けた後、座標系ツリー全体のPDFをカレントフォルダに書き出します。親のない座標系や親が2つある座標系を見つけるには、これが最も手早い方法です。

次に進むには

ROS 2 talker・listener・tf2プロジェクトでは、これらのブロックを、ノードを分けてローンチファイルを備えたパッケージにします。ROSとミドルウェアのハブとROSプラットフォームのページでは、パッケージ、ローンチファイル、パラメータ、そしてこれらの要素の上に構築されたナビゲーションとマニピュレーションのスタックを扱います。

演習

1. listenerのサブスクリプションをQoSProfileでbest-effortの信頼性に変更し、talkerはreliableのままにします。メッセージは引き続き届くでしょうか。次に両者を入れ替え（best-effortのtalker、reliableのlistener）、コマンドライン版に対してros2 topic info -vを実行して理由を確かめてください。
2. tf2のブロックで、レーザの取り付け位置をbase_linkの0.10 m前方・0.05 m左方とし、z軸まわりに0.5 rad回転させます。新しいodomからlaserへの並進を手計算で予測し、ブロックを実行して確かめてください。
3. ループの後でtf_buffer.lookup_transform("laser", "odom", Time())を呼び、odomからlaserへの結果と比較してください。両者の関係を説明してください。

まとめ

• ROS 2のシステムは、名前付きのトピックを介して型付きのメッセージをやり取りするノードのグラフです。Jazzyのデフォルトのミドルウェアは Fast DDSで、ノードは中央サーバなしで互いを発見します。
• デフォルトのQoSはkeep last 10、reliable、volatileです。サブスクリプションは、要求より低い品質しか提供しないパブリッシャからは何も受け取りません。その兆候はログの警告だけです。
• コールバックはエグゼキュータが実行します。上限付きのループでspin_onceを回し、その後ノードを破棄してrclpy.shutdown()を呼べば、自動で終了するプログラムになります。
• tf2は座標系のツリーを保持します。変化する変換はタイムスタンプ付きで/tfに、固定の変換は/tf_staticに1回だけ送られます。BufferとTransformListenerがそれらをつなげ、あらゆる参照に応えます。
• ROSのメッセージは四元数を（x, y, z, w）の順で格納します。新しい座標系ツリーを作ったら、必ず1つの変換を手計算で確かめてください。

さらに読む

• ROS 2 Jazzy Jalisco release notes：対応プラットフォーム、Tier、Ironからの変更点。
• ROS 2 Jazzy documentation, Quality of Service settings：各ポリシー、デフォルトのプロファイル、パブリッシャとサブスクリプションの間の互換性ルール。
• ROS 2 Jazzy documentation, Writing a simple publisher and subscriber (Python)：同じtalkerとlistenerを、別々の実行ファイルを持つパッケージとして作る。
• ROS 2 Jazzy documentation, tf2：座標系ツリーの背後にある考え方。ブロードキャスタとリスナのチュートリアルへのリンク付き。
• REP 105, Coordinate Frames for Mobile Platforms：本レッスンで使うmap・odom・base_linkの慣例。