ROS2で「動いている感覚」をターミナルでつかむ（前回の続き）

はじめに ― 前回からの続きとして

今回は、前回のROS2で「動かす」ために押さえておきたい考え方の続きです。

前回では、ROS2 における座標系、オドメトリ、変換について、
研究的な定義ではなく、エンジニアが「動かす」ための視点で整理しました。

今回は、その前提を踏まえたうえで、
実際にノードを動かしながら挙動を確認する回です。

ただし、まだ rviz（GUIツール） は使いません。
可視化は行わず、ターミナルに表示される数値とログだけを見て進めます。

目的はシンプルに以下です。

cmd_vel の命令と、
ロボット目線での「動いている感覚」が一致していることを体感する。

今回やること・やらないこと

今回やることは、次の3点だけです。

・teleop を使って cmd_vel を送る
・仮想オドメトリで /odom を生成する
・ターミナル上の表示から「どう動いているか」を確認する

一方で、今回は以下のことは行いません。

・rviz（GUI） による可視化
・tf の詳細な説明
・地図（map）
・自律走行や Nav2

あくまで、最小構成で理解を深めることを目的とします。

仮想オドメトリを生成する virtual_odometry_node

まずは、今回の構成の一つの主役である
virtual_odometry_node について説明します。

このノードは、実際のセンサーやモーターを使わずに、
/cmd_vel の命令と時間の経過だけから
仮想的なオドメトリ（/odom）を生成するノードです。

「命令どおりにロボットが完璧に動いたとしたら、
今どこにいるはずか？」を計算していると考えると分かりやすいでしょう。

このような構成にすることで、

• cmd_vel の意味
• オドメトリが何を表しているか
• 座標系の違い

を、センサー誤差などを気にせずに確認できます。

virtual_odometry_node.py（ソースコード）

#!/usr/bin/env python3

import math
import rclpy
from rclpy.node import Node

from geometry_msgs.msg import Twist
from geometry_msgs.msg import Quaternion
from nav_msgs.msg import Odometry

def yaw_to_quaternion(yaw: float) -> Quaternion:
    """
    yaw（ラジアン）から Quaternion を生成する。
    今回は平面移動のみなので roll, pitch は 0。
    """
    q = Quaternion()
    q.x = 0.0
    q.y = 0.0
    q.z = math.sin(yaw / 2.0)
    q.w = math.cos(yaw / 2.0)
    return q

class VirtualOdometryNode(Node):
    """
    /cmd_vel を入力として、仮想的に自己位置 (x, y, theta) を計算し、
    /odom (nav_msgs/Odometry) を publish するノード。
    """

    def __init__(self):
        super().__init__('virtual_odometry_node')

        # ===== 内部状態（自己位置）=====
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0  # yaw [rad]

        # ===== 最新の速度指令 =====
        self.v = 0.0  # linear.x [m/s]
        self.w = 0.0  # angular.z [rad/s]

        # ===== パラメータ =====
        self.dt = 0.1  # 更新周期 [s]（10Hz）

        # ===== Subscriber: /cmd_vel =====
        self.create_subscription(
            Twist,
            '/cmd_vel',
            self.cmd_vel_callback,
            10
        )

        # ===== Publisher: /odom =====
        self.odom_pub = self.create_publisher(
            Odometry,
            '/odom',
            10
        )

        # ===== タイマー =====
        self.create_timer(self.dt, self.update_odometry)

        self.get_logger().info('Virtual Odometry Node started.')

    def cmd_vel_callback(self, msg: Twist):
        """
        /cmd_vel を受信し、最新の速度指令を保存する
        """
        self.v = msg.linear.x
        self.w = msg.angular.z

    def update_odometry(self):
        """
        速度指令を積分して自己位置を更新し、/odom を publish
        """
        # --- オドメトリ計算 ---
        self.x += self.v * math.cos(self.theta) * self.dt
        self.y += self.v * math.sin(self.theta) * self.dt
        self.theta += self.w * self.dt

        # --- Odometry メッセージ作成 ---
        odom = Odometry()
        odom.header.stamp = self.get_clock().now().to_msg()
        odom.header.frame_id = 'odom'
        odom.child_frame_id = 'base_link'

        # 位置
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0.0

        # 姿勢（yaw → quaternion）
        odom.pose.pose.orientation = yaw_to_quaternion(self.theta)

        # 速度（そのまま反映）
        odom.twist.twist.linear.x = self.v
        odom.twist.twist.angular.z = self.w

        # --- publish ---
        self.odom_pub.publish(odom)

        # --- ログ出力 ---
        self.get_logger().info(
            f'x={self.x:.2f}, y={self.y:.2f}, theta={self.theta:.2f}'
        )

def main(args=None):
    rclpy.init(args=args)
    node = VirtualOdometryNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

virtual_odometry_node がやっていること

このノードでは、以下の処理を行っています。

• /cmd_vel を購読する
• ロボット目線の速度（前進速度・角速度）を受け取る
• ロボットの向きを考慮して、ワールド座標での移動量に変換する
• その結果を /odom として配信する

ここで使われている変換式は、
ロボット目線の命令をワールド座標に写し替えている
だけです。

数式の導出を理解する必要はありません。
重要なのは、

• /cmd_vel はロボット目線
• /odom はワールド座標

という役割分担が、コードとしてそのまま表れている点です。

odom_analyzer_node を使って動きを「読む」

odom_analyzer_node は、ROS2 の /odom トピックを購読し、
ロボットが「今どう動いているか」を人間に分かりやすい形で表示するためのノードです。

rviz のような可視化ツールは使わず、
ターミナル上に 1 行で状態を表示します。
これにより、/cmd_vel の命令と、
実際のオドメトリの変化を直接結び付けて確認できます。

このノードでやっている「変換」の意味

/odom に含まれる位置情報（x, y）は、
ワールド座標系で表現されています。
これは、ロボットがどれだけ向きを変えても動かない、
上から見下ろすような固定された座標系です。

一方で、人がロボットの動きを理解するときは、
「前進している」「後退している」「左に回っている」といった
ロボット目線での表現の方が直感的です。

odom_analyzer_node では、
ワールド座標系で表現された移動量（dx, dy）を、
ロボットの向き（theta）を使ってロボット前方向に投影します。

これにより、

• 前進しているのか
• 後退しているのか
• 旋回しているのか（左／右）

を判定しています。

ここで重要なのは、数式そのものではなく、
「ワールド座標の結果を、ロボット目線で解釈し直している」
という点です。

これは、前回で説明した
「変換は数学の問題ではなく、アプリケーションの都合である」
という考え方そのものです。

---

odom_analyzer_node.py（ソースコード）

#!/usr/bin/env python3

import math
import sys
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry

class OdomAnalyzerNode(Node):
    """
    /odom を subscribe し、
    ロボットの向きに対する前進／後退・旋回を投影計算で判定するノード
    """

    def __init__(self):
        super().__init__('odom_analyzer_node')

        self.prev_x = None
        self.prev_y = None
        self.prev_theta = None

        self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10
        )

        self.get_logger().info(
            'Odom Analyzer Node started (projection-based interpretation).'
        )

    def odom_callback(self, msg: Odometry):
        # --- 現在の位置 ---
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y

        # --- quaternion → yaw ---
        q = msg.pose.pose.orientation
        theta = math.atan2(
            2.0 * (q.w * q.z),
            1.0 - 2.0 * (q.z * q.z)
        )

        # 初回は初期化のみ
        if self.prev_x is None:
            self.prev_x = x
            self.prev_y = y
            self.prev_theta = theta
            return

        # --- 差分 ---
        dx = x - self.prev_x
        dy = y - self.prev_y
        dtheta = theta - self.prev_theta

        # --- ロボット前方向への投影 ---
        forward = dx * math.cos(theta) + dy * math.sin(theta)

        # --- 状態判定 ---
        if abs(forward) < 0.005:
            move_str = 'STOP'
        elif forward > 0:
            move_str = 'FORWARD'
        else:
            move_str = 'BACKWARD'

        if abs(dtheta) < 0.02:
            turn_str = 'STRAIGHT'
        elif dtheta > 0:
            turn_str = 'LEFT'
        else:
            turn_str = 'RIGHT'

        # --- 1行ステータス表示 ---
        sys.stdout.write(
            f'\r'
            f'[STATUS] '
            f'x={x:6.2f}  '
            f'y={y:6.2f}  '
            f'theta={theta:6.2f} | '
            f'move={move_str:8s} ({forward:6.3f}m) | '
            f'turn={turn_str:8s} ({dtheta:6.3f}rad)   '
        )
        sys.stdout.flush()

        # 更新
        self.prev_x = x
        self.prev_y = y
        self.prev_theta = theta

def main(args=None):
    rclpy.init(args=args)
    node = OdomAnalyzerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        print()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

パッケージ構成

本記事では、以下のようなパッケージ構成を前提とします。

ros2_ws/
└── src/
    └── virtual_odometry/
        ├── virtual_odometry/
        │   ├── virtual_odometry_node.py
        │   └── odom_analyzer_node.py
        ├── package.xml
        └── setup.py

setup.py のエントリーポイント設定

entry_points={
    'console_scripts': [
        'virtual_odometry_node = virtual_odometry.virtual_odometry_node:main',
        'odom_analyzer_node = virtual_odometry.odom_analyzer_node:main',
    ],
},

ビルド方法

ワークスペースのルートで以下を実行します。

cd ~/ros2_ws
colcon build
source install/setup.bash

ノードが見つからない場合は、
source install/setup.bash を実行しているかを確認してください。

ノードの実行方法（ターミナルを3つ使用）

① 仮想オドメトリノード

ros2 run virtual_odometry virtual_odometry_node

② odom_analyzer_node

ros2 run virtual_odometry odom_analyzer_node

起動すると、ターミナルに 1 行で状態が表示され続けます。

③ teleop_twist_keyboard

ros2 run teleop_twist_keyboard teleop_twist_keyboard

キーボードで前後左右操作するイメージです。

インストールされていない場合は以下のようにインストールします。

apt update
apt install ros-humble-teleop-twist-keyboard

確認ポイント

• 前進操作で move=FORWARD、x が増える
• 後退操作で move=BACKWARD、x が減る
• 左旋回で turn=LEFT、theta が増える
• 右旋回で turn=RIGHT、theta が減る

cmd_vel の命令と、オドメトリの変化が
ロボット目線で一致していることを確認してください。

補足：今回の実行環境について

本記事の内容は、Docker コンテナ上の ROS2 Humble 環境で動作確認しています。

• ホストOS：Windows 11
• 実行環境：WSL2 + Docker
• ROS2：Humble

以下の記事で構築方法等を書いています。
Windows 11 で ROS2 を最速体験する：Docker環境構築ガイド

今回使用しているノードは、センサーや実機ハードウェアに依存しないため、
Linux 環境（PC直インストール、Raspberry Pi、VPS など）でも同様に動作します。

まとめ

今回は、
/cmd_vel と /odom の関係を
ターミナル上だけで確認しました。

ワールド座標で表現されたオドメトリを、
ロボット目線で解釈することで、
「今どう動いているか」を安心して読み取ることができます。

今回はここまでです。また書きたいと思います。
ソース一式は以下で公開しています。