概要

何を試した?

MRで現実空間に座標点を置いたあと、Ubuntu Desktop側で座標を受け取る。
その後UbuntuでTelloドローンを操作し、受け取った座標を経路として動くようにする。
なお、「Telloドローン」の選択に深い理由はない。以前買った私物を転用した。

なぜ？

自動運転するロボットを動かす前に、開発者などがPCで環境情報の設定をする必要がある。
今回、誰でも直感的に環境情報を設定できるMRの実装を試みた。

各業界で本当に必要とされるか(=非開発者が設定したい需要があるか)は未検証。
経路を頻繁に更新したい場合は役に立てるか？

どうやって？

QuestとUbuntuの両方で同じ3枚のAprilTagから同じ座標系を作り、
その座標系のデータでTelloを動かす。

途中経過の動画

上記の流れで進めようとしたが、TelloドローンがすぐスリープになるのとROS2・ドローン処理に慣れないので詰まっている。Quest → Ubuntuの通信には成功したが、Ubuntu ↔︎ Telloドローンのやり取りには成功していない。

Tello以外のハードで、制御しやすく安いのはあまりなかった。
途中ではあるが、記事にして現状の知見を残しておこうと考えた。

QuestでAprilTagを認識し、座標を送信するまで

Ubuntu Desktopからトピックを送信し、Telloドローンを離陸させる(Ubuntu → Tello)

2. 用語

AprilTag：黒白の四角いQRコードのようなマーカー。ARマーカーのようなもの
Anchor：この記事では座標系を決める基準点
Topic：ROS2の掲示板のような通信手法。誰かが貼り、誰かが読む
Message：掲示板の付箋
DDS：掲示板を管理するシステムのような通信基盤

3. 処理のフロー

[Quest]
  パススルー映像でAprilTag(standard 41h12系のA=9, B=14, C=20)を見る
  → 3点の並びからアンカーを作る
  → コントローラで点を置く
  → 点の座標をアンカーの座標系に直し、TCPで送る(PoseArray)

[Ubuntu / ROS2]
  Telloドローンのカメラ映像からAprilTagを認識(同じA/B/C)
  → 同じ手順で同じアンカーを作る
  → 受け取った点とTelloの現在位置をアンカーの座標系で比較
  → Telloドローンへトピックにて回転・移動指令を送る

4. 実装の説明

Quest：3枚のタグからアンカーを作る

// A(tag_9), B(tag_14), C(tag_20)から右手座標を作成
static bool TryBuildFrame(Vector3 A, Vector3 B, Vector3 C, out Pose anchor)
{
    var x  = (B - A).normalized;
    var yp = (C - A).normalized;
    var z  = Vector3.Cross(x, yp).normalized;
    var y  = Vector3.Cross(z, x).normalized;
    
    // 計算で導いたZ前/Y上に近い右手座標系
    anchor = new Pose(A, Quaternion.LookRotation(z, y));
    
    return true;
}

Quest：置いた点をアンカー基準にして送る(TCP→ROS2)

public void PublishVolumes(Transform anchor, IReadOnlyList&lt;Volume&gt; vols)
{
    var pa = new PoseArrayMsg {
        header = new HeaderMsg(new TimeMsg(0,0), "mr_anchor"),
        poses  = new PoseMsg[vols.Count]
    };
    
    for (int i = 0; i &lt; vols.Count; i++)
    {
        var w = vols[i].center;
        
        // アンカーの座標系に転換
        var a = anchor.InverseTransformPoint(w);
        
        // Unityの並び(x,y,z) → ROS2の並び(x,z,y)に入れ替え
        pa.poses[i] = new PoseMsg(new PointMsg(a.x, a.z, a.y), new QuaternionMsg(0,0,0,1));
    }
    
    conn.Publish("/mr/volumes/centers", pa);
}

Ubuntu：TelloカメラでAprilTagを認識し、同じアンカーを作る(ここからうまく試せていない)

# 目標と現状の位置・向きの差分を計算
ex, ey, ez = target.x - now.x, target.y - now.y, target.z - now.z
yaw_to = atan2(ey, ex)
eyaw   = wrap_angle(yaw_to - now.yaw)

# 目標に向けて旋回後、前進と高さ移動で目標へ向かうための計算
ang_z  = clamp(k_ang * eyaw, -max_w, max_w)
gate   = 1.0 if abs(eyaw) < math.radians(45) else max(0.0, 1.0 - (abs(eyaw)-math.radians(45))/math.radians(45))
lin_x  = clamp(k_lin * math.hypot(ex,ey), 0, max_v) * gate
lin_z  = clamp(k_vert* ez, -max_vz, max_vz)

# トピックでTelloを制御
publish('/control', Twist(x=lin_x, z=lin_z, yaw=ang_z))

5. 実装でつまづいたところ

UnityのROS Protocol選択欄を見逃していた

一生繋がらないと思ったら、UnityのProtocol設定がROS1だった。
上部ツールバーのRobotics > ROS SettingsからProtocolを変えられる。
ROS1は研究目的で作られたが、ROS2は産業向けに作られていて、仕様の差異が大きいらしい。

パススルーの映像にアクセスするためのWebCameraTextureの制限

パススルー映像を使ってAprilTagの検出をしたが、その際UnityのWebCameraTexture経由でデータを取得していた。
しかし、Unityの制限でWebCameraTexture経由では左右どちらかのカメラ1本の映像しか取得できなかった。2本同時に取得して、座標系計算に使うことはできなかった。
ネイティブ実装では2本同時に取得出来るらしいが試していない。

QuestにてパススルーとAprilTag検出の両立

当初、パススルーとAprilTag検出の両立に苦戦していた。
記憶が曖昧で申し訳ないが、片方に成功すれば片方に失敗するような事象があった気がする。
結果的にはパススルーをOpenXR経由、AprilTag検出をWebCameraTexture経由にして、処理を分けたことで両立ができた。

ドローンが飛行中安定しない

ドローンをあまり触ってきておらず、そんなにブレないものかと思っていた。
実際には一度離陸したら、ドローンはかなり動いてしまう。
このことはAprilTag検出にも移動指示にも影響する。
手元にドローンしかなかったが、この実装はロボット掃除機や台車の方が向いていた。

6. 主な使用ライブラリ

Quest側

AprilTag package for Unity
ROS TCP Connector(0.7.0-preview)

Ubuntu Desktop側

ROS2 Jazzy
apriltag_ros
ROS TCP Endpoint
DJI Tello ROS2
DJITelloPy

7. 所感

ロボット掃除機で開発する機会があれば、修正して試してみたい
ただ、MRでAprilTag検出した際にアンカーがTag9(原点)よりずれていたため、カメラの物理スペックを基にした補正処理も必要そう
また、現場ではかなり離れた距離に座標を配置することもあるはずなので、この実装ではAprilTagを離れた位置に置いて連携させるなど工夫が必要となる
そもそも現場で何が課題でXRがどう関われそうか自体も機会があれば知りたい