LOG31R2R2R2026/1/9

実験3-ROS 2とUnityを介した双方向リアルタイム・テレオペレーションの確立

実験3の目的

自作のPythonプログラムから、ROS 2通信プロトコルを用いて、Unity上のデジタルツイン（mechArm 270 M5）の各関節をキーボード入力によりリアルタイム操作する。

※本プロジェクトの目的：実機「mechArm 270 M5」の制御を通じて、AIの判断をいかに物理的な動きへ変換するか。そのロボット制御の仕組みを理解するプロセスを記録・検証するものです。

制御プログラムの進化と技術的ブレイクスルー

本実験の核心は、Pythonによるキー入力監視と、ROS 2の非同期通信をいかに衝突させずに共存させるかにあった。

初期版の課題：特殊キーによるロジックの停止

当初のプログラムは、数字キー（文字属性）の取得のみを想定した単純な構造であった。そのため、矢印キー（特殊キー属性）が押された瞬間にプログラム内部で例外が発生し、命令のパブリッシュ（送信）処理まで到達できないという論理の断絶が起きていた。

エラーの正体: key.char を参照する際、矢印キーにはその属性が存在しないため AttributeError が発生。
第5の窓（Echo）の沈黙: プログラムは動いているように見えても、内部エラーによりデータがROS 2ネットワークへ一度も送り出されていなかった。

修正した制御用プログラム

以下のmanual_control.pyコードでは、「堅牢な入力判定」と「通信の強制処理（Spin）」という2つの重要な改良を施した。

Python

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from pynput import keyboard

class ManualControlNode(Node):
    def __init__(self):
        super().__init__('manual_control_node')
        # 通信の出口を定義
        self.pub = self.create_publisher(JointState, '/arm_joints', 10)
        self.msg = JointState()
        self.msg.name = ['link1', 'link2', 'link3', 'link4', 'link5', 'link6']
        self.current_pos = [0.0] * 6 # 現在の全関節角度を保持
        self.selected_joint = 0 # 選択中の関節インデックス
        print(f"--- 操縦開始 (1-6で関節選択, 矢印で移動) ---")

    def publish_joints(self):
        """現在の角度情報をROS 2トピックへパブリッシュする"""
        self.msg.position = self.current_pos
        self.pub.publish(self.msg)

    def on_press(self, key):
        """
        キー入力時のコールバック。
        数字キー（関節選択）と特殊キー（矢印操作）を分離して処理する。
        """
        # A. 数字キーの判定 (try-exceptでAttributeErrorを封じ込める)
        try:
            if hasattr(key, 'char') and key.char is not None:
                if '1' <= key.char <= '6':
                    self.selected_joint = int(key.char) - 1
                    print(f"選択中: link{self.selected_joint+1}")
        except: 
            pass # 特殊キーの場合はここをスルーして下の判定へ進む

        # B. 矢印キーの判定 (keyboard.Keyオブジェクトを直接比較)
        if key == keyboard.Key.left:
            self.current_pos[self.selected_joint] += 0.1
            print(f"移動中: link{self.selected_joint+1} (+0.1)")
            self.publish_joints() # 命令を送信
        elif key == keyboard.Key.right:
            self.current_pos[self.selected_joint] -= 0.1
            print(f"移動中: link{self.selected_joint+1} (-0.1)")
            self.publish_joints() # 命令を送信
        elif key == keyboard.Key.esc:
            return False

def main():
    rclpy.init()
    node = ManualControlNode()
    
    # C. ROS 2通信の「押し出し」処理の実装
    # キー監視（Listener）を別スレッドで回しつつ、
    # メインループで spin_once を呼び出し、パブリッシュを確実に実行させる
    with keyboard.Listener(on_press=node.on_press) as listener:
        while listener.running and rclpy.ok():
            rclpy.spin_once(node, timeout_sec=0.1) # 0.1秒ごとに通信スタックを処理
    
    rclpy.shutdown()

if __name__ == '__main__':
    main()

修正の要点と意義

例外処理の導入: hasattr と try-except を組み合わせることで、特殊キー押下時にもメインの処理が中断されない「止まらないコード」へと進化させた。
rclpy.spin_onceの採用: ROS 2では publish() を呼ぶだけではデータが送信待ちキューに留まることがある。この一行を加えることで、OSの通信機能へデータを強制的に流し込み、リアルタイム性を確保した。
状態の視覚化（Printデバッグ）: 操作中に 選択中 移動中 というログをターミナルに出力するようにしたことで、ユーザーの操作がプログラムに受理されたかを即座に判断できるUI/UXを実現した。

トラブルシューティングの全プロセス

① セキュリティ権限の攻防

状況: ターミナルに This process is not trusted! と表示され、キーボード操作が一切効かない。
やり取り: 「入力監視」の設定だけでは不十分であることが判明。
解決: プライバシーとセキュリティ内のアクセシビリティに、ターミナルを手動（＋ボタン）で追加し、スイッチをオンにすることで、PythonがOSレベルで入力を監視できるようになった。

② トピック名のスペルミス（mとn）

状況: Unity画面に赤いエラーが並び、通信が確立されない。
やり取り: ログを確認したところ、Endpointが /arn_joints という存在しない名前を探していた。
解決: Unityの JointStateSubscriber の Topic Name を、Python側の指定と一致する /arm_joints に修正した。

③ 矢印キーの論理エラー

状況: 右下ターミナルに ^[[C などの記号が出るだけで、ロボットが動かない。
やり取り: pynput が文字以外のキーを判定する際にエラーを吐き、送信処理がスキップされていた。また、命令をネットワークへ送り出す spin_once が不足していた。
解決: 特殊キーを try-except の外で判定する構造に修正し、通信ループを追加したコードへ刷新した。

④ Unity内の制御権競合

状況: 第5の窓（Echo）で数値が流れているのに、Unity上のロボットが反応しない。
やり取り: Unity内に標準で付いている Controller (Script) がオンのままであった。
解決: インスペクター上のチェックを外して無効化。これにより、Unityの自律制御が止まり、ROS 2からの外部命令が物理エンジンを動かせるようになった。

システム構成図（データフロー）の詳細解説

本システムは、以下の5つのステップを経てリアルタイムな双方向通信を実現しています。

制御フロー（Outbound：命令の送信）

入力層（Local）: 右下ターミナルの manual_control.py が pynput を介してキーボードの打鍵（矢印キーなど）を検知する。
命令生成層（ROS 2）: 検知した入力を角度情報（JointState メッセージ）に変換し、ROS 2トピック /arm_joints へパブリッシュ（発行）する。
ブリッジ層（TCP）: 左上ターミナルの ros_tcp_endpoint が、ROS 2ネットワーク内の命令をキャッチし、TCP通信によってUnityへ転送する。
実行層（Unity）: Unity内の JointStateSubscriber が命令を受信。Articulation Body（物理エンジン）が指定された角度へロボットアームを駆動させる。
※この際、Controller (Script) をオフにすることで、Unity内部の固定制御をバイパスし、外部命令を優先させている。

フィードバックフロー（Inbound：状態の返報）

計測層（Unity）: Unity内の JointStatePublisher が、物理演算の結果動いたロボットの「現在の真の角度」を毎フレーム取得する。
返報層（TCP/ROS 2）: UnityからTCP経由で ros_tcp_endpoint へデータが送られ、ROS 2トピック /joint_states として再発行される。
可視化層（Monitoring）:
右上ターミナル: joint_monitor_all.py が数値を読み取り、リアルタイム・グラフとして描画する。
左下ターミナル: ros2 topic echo が生データを数値としてスクロール表示し、通信の整合性を証明する。

追加実装：Unity HUD（画面内情報表示）の構築

シミュレーションの利便性を高めるため、外部ターミナルに頼らずUnity画面内で完結する管制用HUDを実装した。

プロセスと実装内容

UIレイアウトの構築: Unity 6のCanvas Scalerを活用し、解像度に依存しないUIの土台を作成。TextMeshProを採用し、視認性の高いグリーン（#00FF00）の文字でステータスを表示させた。
データの橋渡し（スクリプト連携）:

HudManager.cs を新規作成し、受け取ったラジアン値をフォーマットしてテキスト表示する機能を実装。
既存の JointStateSubscriber.cs に HudManager への参照を追加し、ROS 2メッセージを受信するたびにUIを更新する「連動回路」を構築した。

実装後の効果

認知負荷の激減: 視線をターミナルに落とすことなく、ロボットの挙動と現在の関節角度を一目で把握可能になった。
接続状況の可視化: 通信が確立されるとROS2 SYSTEM: CONNECTEDと表示されるようになり、デバッグの効率が向上した。

実験4：速度制御による操作性の極致

実験3の「位置制御（ステップ移動）」を卒業し、より実用的で滑らかな速度制御へとシステムをアップグレードした。

プロセスと技術的ブレイクスルー

「連打」から「長押し」へのパラダイムシフト

pynput の on_release（キーを離した時）イベントを追加実装。
キーを押している間は速度をセット、離すとゼロにするというフラグ管理ロジックを導入した。

高精度なイベントループの構築:

while ループ内で rclpy.spin_once を 0.03〜0.05秒間隔で高速回転させ、速度フラグがオンの間、常に角度を微加算し続ける「自律送信」機能を実装した。

実験結果の分析

データの連続性: ros2 topic echo 上で、数値が「1.45 ➔ 1.40」と流れるように変化することを確認。これは通信がパルス状ではなく、安定したストリーム（流れ）になったことを意味する。
物理挙動の安定: ロボットがカクカクとテレポートするのではなく、スーッと加速・移動・停止する滑らかな挙動へと進化した。