Tech blog Produced by FOURIER

mocopiの通信データパースライブラリを作ってみた

Hirayama Hirayama 2023.04.21

はじめに

今年の 1 月末にmocopiが発売されてから、ネット上で様々なmocopiを使った記事や動画が投稿されています。

弊社でも発売開始直後に購入していたのですが、しばらく放置中々記事にすることができず、早数ヶ月経過してしまいました。ただ、せっかく購入したのに何もしないのはもったいないため、遅ればせながらmocopiを使って色々なことをしていきたいと思います。

今回は、まずmocopiから送信されてくるデータのパーサーを作り、今後の開発をスムーズにできるようにできるようなライブラリを作っていこうと思います。

実装について

実装例がないか探してみたところ、公式からパースするためのライブラリはなかったものの、以下の有志の方が作成したコードがありました。

この方の実装言語は Python でしたが、自分の予定では RaspberryPi や Arduino などのマイコンと連携していきたいと思っているので、Rustで実装しました。

調査

いざ実装を始めて見たものの、そもそも UDP レシーバーを実装したことがなかったため、実際にmocopiの通信を読み取り、Wiresharkを使用してどのようなデータが受信できるか見てみるところから始めました。

mocopi からデータを受信する

まずは UDP 経由でデータを受信できるのか確かめます。

説明書どおりにmocopiを体に装着し、モーションキャプチャ画面まで表示します。

そして、PC の IP アドレスに向けて UDP を送信するように設定し、送信します。

上手くいくと、以下のように PC でデータを受信できます。

wiresharkを使用して受信したデータを確認

mocopiデータをパース

前節でデータを送信の確認ができたので、これと解析レポートを見ながら、このデータをプログラム上で受信し、データを利用しやすいようにパースする処理を書いていきます。

普段はRustを書かないので、最初はかなり苦戦しましたが、以下のブログで紹介されていた nom というライブラリを使用することで、何とか実装することができました。

以下が実装したコードです。構造体の定義を見るとわかりますが、元データの構造から若干変えているところがあります。

use std::{env};
use std::io::{Cursor};
use std::net::{UdpSocket};
use local_ip_address::local_ip;
use nom::bytes::complete::take;
use nom::number::complete::{le_u32};
use nom::error::Error;

type BoneId = u16;
type TransVal = f32;

#[derive(Debug, PartialEq)]
pub struct SkeletonPacket {
    pub head: Head,
    pub info: Info,
    pub skeleton: Skeleton,
}

#[derive(Debug, PartialEq)]
pub struct Head {
    pub format: String,
    pub ver: u8,
}

#[derive(Debug, PartialEq)]
pub struct Info {
    pub addr: u64,
    pub port: u16,
}

#[derive(Debug, PartialEq)]
pub struct Skeleton {
    pub bones: Vec<Bone>,
}

#[derive(Debug, PartialEq)]
pub struct Bone {
    pub id: BoneId,
    pub parent: BoneId,
    pub trans: Transform,
}

#[derive(Debug, PartialEq)]
pub struct FramePacket {
    pub head: Head,
    pub info: Info,
    pub frame: Frame,
}

#[derive(Debug, PartialEq)]
pub struct Frame {
    pub num: u32,
    pub time: u32,
    pub bones: Vec<BoneTrans>,
}

#[derive(Debug, PartialEq)]
pub struct BoneTrans {
    pub id: BoneId,
    pub trans: Transform,
}

#[derive(Debug, PartialEq)]
pub struct Transform {
    pub rot: Rotation,
    pub pos: Position,
}

#[derive(Debug, PartialEq)]
pub struct Rotation {
    pub x: TransVal,
    pub y: TransVal,
    pub z: TransVal,
    pub w: TransVal,
}

#[derive(Debug, PartialEq)]
pub struct Position {
    pub x: TransVal,
    pub y: TransVal,
    pub z: TransVal,
}

#[derive(Debug, PartialEq)]
pub struct Data<'a> {
    pub len: u32,
    pub name: String,
    pub data: &'a [u8],
    pub rem: &'a [u8],
}

fn parse_value(data: &[u8]) -> Data {
    // lengthの長さは4bytesで固定
    let (data, length) = le_u32::<_, Error<_>>(data).unwrap() as (&[u8], u32);

    // nameは4bytesの文字列
    let (data, name) = take::<_, _, Error<_>>(4usize)(data).unwrap();
    let name_str = String::from_utf8(name.to_vec()).unwrap();

    // valueの長さはlengthの値による
    let (rem, data) = take::<_, _, Error<_>>(length)(data).unwrap();

    return Data {
        len: length,
        name: name_str,
        data,
        rem,
    };
}

fn parse_head(data: &[u8]) -> (u32, Head) {
    let data = parse_value(data);
    let len = data.len;

    // ftyp
    let data= parse_value(data.data);
    let format = String::from_utf8(data.data.to_vec()).unwrap();

    // vrsn
    let data = parse_value(data.rem);
    let ver = data.data[0];

    (len, Head { format, ver })
}

fn parse_info(data: &[u8]) -> (u32, Info) {
    let data = parse_value(data);
    let len = data.len;

    // ipad
    let data = parse_value(data.data);
    let addr = u64::from_le_bytes(data.data.try_into().unwrap());

    // rcvp
    let data = parse_value(data.rem);
    let port = u16::from_le_bytes(data.data.try_into().unwrap());

    (len, Info { addr, port })
}

fn parse_skeleton(data: &[u8]) -> (u32, Skeleton) {
    // skdf
    let data = parse_value(data);
    let len = data.len;

    // bons
    let (_, bones) = parse_bones(data.data);

    (len, Skeleton { bones: *bones })
}

fn parse_frame(data: &[u8]) -> (u32, Frame) {
    // fram
    let data = parse_value(data);
    let len = data.len;

    // fnum
    let data = parse_value(data.data);
    let num = u32::from_le_bytes(data.data.try_into().unwrap());

    // time
    let data = parse_value(data.rem);
    let time = u32::from_le_bytes(data.data.try_into().unwrap());

    // btrs
    let (_, bones) = parse_bone_trans(data.rem);

    (len, Frame { num, time, bones: *bones })
}

fn parse_bone_trans(data: &[u8]) -> (u32, Box<Vec<BoneTrans>>) {
    // btrs
    let btrs_data = parse_value(data);
    let btrs_len = btrs_data.len;

    // btrsの下にあるbtdtをparseしていく
    let mut bones: Vec<BoneTrans> = Vec::new();
    let mut read_bytes: u32 = 0;
    loop {
        let part = &btrs_data.data[(read_bytes as usize)..];

        // btdt
        let data = parse_value(part);
        let len = data.len;

        // bnid
        let data = parse_value(data.data);
        let id = u16::from_le_bytes(data.data.try_into().unwrap());

        // tran
        let (_, trans) = parse_trans(data.rem);

        bones.push(BoneTrans { id, trans });

        read_bytes += len + 8;
        if read_bytes == btrs_len {
            break;
        }
    }

    (btrs_len, Box::new(bones))
}

fn parse_bones(data: &[u8]) -> (u32, Box<Vec<Bone>>) {
    // bons
    let bons_data = parse_value(data);
    let bons_len = bons_data.len;

    // bonsの下にあるbndtをparseしていく
    let mut bones: Vec<Bone> = Vec::new();
    let mut read_bytes: u32 = 0;
    loop {
        let part = &bons_data.data[(read_bytes as usize)..];

        // bndt
        let data = parse_value(part);
        let len = data.len;

        // bnid
        let data = parse_value(data.data);
        let id = u16::from_le_bytes(data.data.try_into().unwrap());

        // pbid
        let data = parse_value(data.rem);
        let parent = u16::from_le_bytes(data.data.try_into().unwrap());

        // tran
        let (_, trans) = parse_trans(part);

        bones.push(Bone { id, parent, trans });

        read_bytes += len + 8;
        if read_bytes == bons_len {
            break;
        }
    }

    (bons_len, Box::new(bones))
}

fn parse_trans(data: &[u8]) -> (u32, Transform) {
    // tran
    let data = parse_value(data);
    let len = data.len;

    // 28bytesのデータを4bytesごとに取り出す
    let mut values = [0.0; 7];
    for i in 0..6usize {
        let v = data.data[i * 4..(i * 4 + 4)].to_vec();
        values[i] = f32::from_le_bytes(v.try_into().unwrap());
    }

    (len, Transform {
        rot: Rotation { x: values[0], y: values[1], z: values[2], w: values[3] },
        pos: Position { x: values[4], y: values[5], z: values[6] },
    })
}

fn main() -> () {
    let args: Vec<String> = env::args().collect();
    let local_ip = local_ip().unwrap();
    let port = match args.get(1) {
        Some(s) => s.clone(),
        None => String::from("12351"),
    };
    let addr = format!("{:?}:{}", local_ip, port);

    let socket = UdpSocket::bind(&addr).expect("couldn't bind socket");
    println!("Successfully {} binding socket", &addr);
    println!("Listening...");

    let mut buff = Cursor::new([0u8; 2048]);

    loop {
        socket.recv_from(buff.get_mut()).expect("didn't receive data");

        let mut data: &[u8] = buff.get_ref();

        let (len, head) = parse_head(data);
        data = &data[((len + 8) as usize)..];

        let (len, info) = parse_info(data);
        data = &data[((len + 8) as usize)..];

        let name= parse_value(data).name;

        if name == "skdf" {
            let (_, skeleton) = parse_skeleton(data);
            let packet = SkeletonPacket { head, info, skeleton };

            dbg!(&packet);
        } else {
            let (_, frame) = parse_frame(data);
            let packet = FramePacket { head, info, frame };

            dbg!(&packet);
        }
    }
}

実行すると、以下のようにコンソールに出力されます。

framパケットの例

パッケージ化

せっかくなので、ここまで来たらパッケージ化まで進めようということで、Cargo パッケージとして公開しました。もしよかったら使ってみてください。

https://crates.io/crates/mocopi_parser

パッケージのソースコードは会社の GitHub にリポジトリにプッシュしたので、そこで確認できます。

まとめ

今回はRustmocopiの受信データをパースするライブラリを作りました。

今のところこのライブラリを使ってやろうとしている案は、

  • mocopiでゲームをプレイする
  • mocopiでロボットを操作する

の二つを考えているので、アイデアが固まったらドンドン作っていこうと思います。

ちなみに、Arduino や RaspberryPi は C 言語での実装経験はあるものの、Rustはコンパイルできると聞いたことがある程度の知識なので、もしかしたらこのライブラリは使えないかもしれないです。🤷‍♂️

Hirayama

Hirayama / Engineer

1997年生まれ、南伊豆出身。学生時代にC#で画像処理アプリケーションを作ったりしていました。業務では主にLaravelを使用してサーバーサイドのプログラミングをしています。趣味はドライブとシミュレーションゲーム。