IMU（bxi_imu）

bxi_imu は ROS 2 を通じて IMU データを配信します。アプリケーションは共通のトピックをサブスクライブすることで、姿勢、角速度、線形加速度を取得できます。

1. インターフェース概要

アプリケーションは、配信されるメッセージのフィールドと座標フレームを使用します。ロボットがすでに有効な IMU データを提供している場合は、既存のトピックを直接サブスクライブしてください。

2. 出力トピック

オプションのメッセージは、デプロイ設定と利用可能なデータフィールドに依存します。トピックがグラフ上に表示されていても、メッセージが継続的に送信されているとは限りません。実際にメッセージを受信できることを確認してください。

3. 環境と起動

新しいターミナルを開くたびに、ROS 環境とプロジェクト環境を読み込みます。この例では標準的な ROS 2 Humble を使用しています。ワークスペースのパスは実際のデプロイ先に置き換えてください。ロボットにプリインストールされた ROS 環境を使用する場合は、1 行目を source /opt/bxi/bxi_ros2_pkg/setup.bash に置き換えてください。

source /opt/ros/humble/setup.bash
cd ~/bxi_ws/bxi_controller_ros2
source install/setup.bash
ros2 pkg prefix bxi_imu

まだビルドしていない新規のソースデプロイでは、ROS を読み込んだ後、install/setup.bash を読み込む前に次を実行してください。

colcon build --merge-install --symlink-install --packages-select bxi_imu

まず既存のデータを確認します。

ros2 topic info -v /hardware/imu_data
ros2 topic echo --once --qos-reliability best_effort \
  /hardware/imu_data sensor_msgs/msg/Imu

echo --once は実際のメッセージを 1 件受信するまで待機します。待機を中止するには Ctrl+C を押してください。IMU を単独で起動するには、次を実行します。

ros2 launch bxi_imu imu.launch.py

制御プログラムが IMU の起動を管理している場合は、その既存トピックを使用してください。IMU データを読み取るだけであれば、ロボット制御プログラム全体を起動する必要はありません。

4. メッセージフィールドと単位

4.1 メイン IMU メッセージ

/hardware/imu_data は sensor_msgs/msg/Imu を使用します。

4.2 オプションメッセージ

5. 座標方向とマッピング

配信されるメッセージは header.frame_id で座標フレームを示し、デフォルトは imu_link です。軸のマッピングはデプロイ設定によって決まり、アプリケーションはマッピング適用済みのデータを受信します。

5.1 identity の意味

axis_mapping が identity の場合、IMU の元の軸方向がそのまま保持されます。

output_x = imu_x
output_y = imu_y
output_z = imu_z

軸の入れ替えや反転は行われず、取り付け角度も自動補正されません。取り付けた IMU の軸がすでに目的の出力方向と一致している場合に使用します。

5.2 その他のマッピング例

例えば、axis_mapping を -y,x,z に設定した場合は次の意味になります。

output_x = -imu_y
output_y =  imu_x
output_z =  imu_z

マッピングは角速度、加速度、磁場、姿勢に一貫して適用されます。オイラー角は変換後の姿勢から再計算されます。出力フレームを使用するアプリケーションでは、同じマッピングを重ねて適用しないでください。

6. QoS と通信環境

パブリッシャは SensorDataQoS を使用します。コマンドラインでのサブスクライブには best_effort を、Python でのサブスクライブには qos_profile_sensor_data を使用してください。

ros2 topic echo --once --qos-reliability best_effort \
  /hardware/imu_data sensor_msgs/msg/Imu

コントローラサービスでロボットを起動している場合は、サブスクライバ側のターミナルでそのサービスの ROS_DOMAIN_ID、RMW_IMPLEMENTATION、DDS ネットワーク設定に合わせてください。詳しくは「システム設定」ページを参照してください。

ホストをまたいでサブスクライブする場合は、ネットワークインターフェースと DDS の接続性も確認してください。

7. Python サブスクライバの例

次のコードを read_imu.py として保存します。メッセージを継続的に受信し、最新データを 1 秒に 1 回出力するとともに、コールバックの到着時刻を確認して 1 秒を超える途切れを検出します。この 1 秒のしきい値はサンプル表示用のものです。

import time

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Imu


class ImuReader(Node):
    def __init__(self):
        super().__init__("imu_reader")
        self.latest = None
        self.last_received = None
        self.subscription = self.create_subscription(
            Imu, "/hardware/imu_data", self.on_imu, qos_profile_sensor_data
        )
        self.timer = self.create_timer(1.0, self.report)

    def on_imu(self, msg):
        self.latest = msg
        self.last_received = time.monotonic()

    def report(self):
        if self.latest is None:
            self.get_logger().info("Waiting for IMU data")
            return
        age = time.monotonic() - self.last_received
        if age > 1.0:
            self.get_logger().warning(f"No IMU update for {age:.2f} s")
            return
        q = self.latest.orientation
        w = self.latest.angular_velocity
        a = self.latest.linear_acceleration
        self.get_logger().info(
            f"frame={self.latest.header.frame_id} "
            f"q(x,y,z,w)=({q.x:.4f},{q.y:.4f},{q.z:.4f},{q.w:.4f}) "
            f"gyro(rad/s)=({w.x:.3f},{w.y:.3f},{w.z:.3f}) "
            f"acc(m/s^2)=({a.x:.3f},{a.y:.3f},{a.z:.3f})"
        )


def main():
    rclpy.init()
    node = ImuReader()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()

実行環境を読み込んでから実行します。

python3 read_imu.py

8. データの有効性と配信レート

ノードは配信前に、クォータニオンの全成分が有限値であること、およびノルムが許容範囲内であることを確認します。デフォルトの範囲は 0.9..1.1 です。チェックに失敗したサンプルは配信されません。合格したクォータニオンは自動的には正規化されず、またこのチェックはその他のセンサフィールドをすべて検証するものではありません。

アプリケーションは受信時刻を記録し、データが継続して更新されているかを検出してください。受信レートは次のコマンドで確認できます。

ros2 topic hz /hardware/imu_data

出力レートは、現在のデバイス設定と実行時の状況に依存します。サブスクライバのコードで固定レートを前提にしないでください。ros2 topic hz が示すのはサブスクライバ側の受信レートであり、ホストのスケジューリングや DDS 通信の影響を受けます。

起動時のタイムアウトメッセージは診断に役立ちます。アプリケーションでの鮮度チェックには、コールバックの受信時刻を使用してください。header.stamp はホストのデコード時刻の分析には役立ちますが、ハードウェアのサンプリング時刻を表すものではありません。

9. コントローラにおける IMU 保護

本節は、bxi_example_py_elf3 の example_demo_hw.launch.py で起動される制御プログラム bxi_example_py_elf3_demo に基づいています。このプログラムは /hardware/imu_data をサブスクライブし、受信データを独自に検証して、異常発生時には制御状態を制限します。以下の起動時待機および実行時の途絶保護は、topic_prefix が hardware/ で始まる場合に有効になります。その他のプログラムエントリーポイントについては個別に確認してください。

9.1 受信データの検証

コントローラは、受信した IMU メッセージごとに次の項目を確認します。

• クォータニオンの 4 成分がすべて有限値であり、ノルムが 0.9..1.1 の範囲内であること。
• 角速度と加速度の 6 成分がすべて有限値であること。

無効なフレームは破棄され、制御用の観測値や最後に有効な IMU を受信した時刻は更新されません。dropping invalid IMU frame 警告には累積件数が含まれ、5 秒間隔に間引いて出力されます。

9.2 起動前の有効データ待機

ハードウェア制御の起動には、これらのチェックに合格した IMU フレームが少なくとも 1 件必要です。それまでの間、プログラムはロボットのリセットや制御起動に進まずに待機します。

デフォルトでは、2 秒ごとに waiting for IMU data を出力し、15 s 経過後に IMU startup timeout エラーを 1 回出力します。この起動タイムアウトはエラーを記録するだけで、プログラムはチェックをスキップしたり自動終了したりせず、待機を続けます。後続の起動ステップでは、関節データなどの条件も必要です。

9.3 実行時の途絶検出

コントローラは、ホストのモノトニッククロックで有効フレームの到着時刻を記録します。各制御周期で入力を読み取る前に、次の条件を確認します。

現在のモノトニック時刻 - 最後の有効受信時刻 ≥ imu_runtime_timeout_sec

デフォルトのしきい値は 0.1 s（100 ms）です。しきい値に達すると、プログラムは IMU data timeout を出力し、異常をラッチして com.bxi.basic_actions/imu_protection を強制的に要求します。

チェックは制御周期に合わせて実行されるため、実際の応答遅延はスケジューリングの影響を受けます。検出には header.stamp ではなく、有効メッセージのコールバック到着時刻を使用します。7 節のサブスクライバ例にある 1 秒の表示用しきい値は、コントローラの保護しきい値とは別のものです。

9.4 保護時の指令と復帰

IMU 途絶保護に入ると、その状態はコントローラが現在記録している関節位置を取得し、次の関節指令を継続的に生成します。

コードには release_imu_protection() メソッドが用意されていますが、/imu_protection_allow_release=false によりデフォルトで無効になっています。明示的に有効化し、かつ有効な IMU データがタイムアウトしきい値内で届き続けている場合、このメソッドを呼び出すとラッチの解除と状態の切り替えを試みます。デフォルトの遷移先は com.bxi.basic_actions/zero_torque です。これはプロセス内のメソッドであり、現時点では対応する単独の ROS 復帰サービスはありません。

9.5 歩行中の傾斜保護

normal 歩行状態、およびその更新ロジックを継承する forward_back 状態では、IMU の姿勢からロールとピッチも計算します。いずれかの絶対角が π/3（60°）を超えると、com.bxi.basic_actions/zero_torque を要求します。

9.6 パラメータ、起動方法と適用範囲

現在の example_demo_hw.launch.py は、imu_runtime_timeout_sec を launch 引数として公開しています。例えば、IMU データソースを別に用意してハードウェアとコントローラ全体を起動する場合は次のとおりです。

ros2 launch bxi_example_py_elf3 example_demo_hw.launch.py \
  enable_imu:=false imu_runtime_timeout_sec:=0.1

このコマンドは単独の bxi_imu インスタンスを起動しません。別のターミナルで IMU データを供給するか、既存のロボット起動フローを使用してください。表中のその他のコントローラノードパラメータは、この launch ファイルでは個別の引数として公開されていないため、コントローラノードのパラメータ設定で指定してください。保護ゲインは基本動作（basic actions）の状態設定に属します。設定は初期化時に読み込まれるため、変更後はコントローラを再起動してください。

10. CSV 記録と実行ログ

起動時に CSV 記録を制御します。

# Follow deployment settings
ros2 launch bxi_imu imu.launch.py imu_record_enabled:=auto

# Disable recording
ros2 launch bxi_imu imu.launch.py imu_record_enabled:=false

# Enable recording
ros2 launch bxi_imu imu.launch.py imu_record_enabled:=true

必要に応じていずれか 1 つのコマンドを実行してください（上から順に、デプロイ設定に従う／記録無効／記録有効）。記録のオン・オフは ROS メッセージのフィールドには影響しません。

CSV には次のデータが含まれます。

CSV には、クォータニオン検証に失敗して ROS に配信されなかったサンプルも含まれる場合があり、フレーム欠落の診断に役立ちます。記録キューの満杯や書き込み失敗により、データに欠落が生じることがあります。プロセスの実行中に、サイズや時間によるファイルのローテーションは行われません。

11. トラブルシューティング

まず /hardware/imu_data で実際にメッセージを受信できることを確認し、その後フィールド、座標方向、受信レートを検証してください。