Produced by FOURIER

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

HirayamaHirayama calender 2023.4.21

はじめに

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

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

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

実装について

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

GitHub - seagetch/mcp-receiver: Open source implementation of receiver plugin for mocopi motion tracking system.

Open source implementation of receiver plugin for mocopi motion tracking system. - GitHub - seagetch/mcp-receiver: Open source implementation of receiver plugin for mocopi motion tracking system.

https://github.com/seagetch/mcp-receiver

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

調査

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

mocopi からデータを受信する

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

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

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

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

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

mocopi データをパース

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

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

Rustでバイナリを読み書きするのに必要なクレート3選 - aptpod Tech Blog

研究開発グループの大久保です。 当社の製品の中にはC/C++で書かれたものが存在し、その中には独自のバイナリフォーマットを取り扱うものが存在します。既存のコードとやり取りするようなRustのプロジェクトを起こすためには、その独自のバイナリフォーマットをRustで取り扱えるようにしなければなりません。しかしながら、Rustの標準ライブラリの機能だけでは、バイナリの読み書きは意外と面倒になります。そのため、今回はRustでバイナリを扱うのならぜひ知っておきたいクレートを3つご紹介します。

https://tech.aptpod.co.jp/entry/2020/10/09/090000

GitHub - rust-bakery/nom: Rust parser combinator framework

Rust parser combinator framework. Contribute to rust-bakery/nom development by creating an account on GitHub.

https://github.com/rust-bakery/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 パッケージとして公開しました。もしよかったら使ってみてください。

mocopi_parser - crates.io: Rust Package Registry

A parser of streamed data from mocopi

https://crates.io/crates/mocopi_parser

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

GitHub - FOURIER-Inc/mocopi-parser: a parser library for streamed data from mocopi.

a parser library for streamed data from mocopi. Contribute to FOURIER-Inc/mocopi-parser development by creating an account on GitHub.

https://github.com/FOURIER-Inc/mocopi-parser

まとめ

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

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

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

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

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

新しいメンバーを募集しています

Hirayama

Hirayama ／ Engineer

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

関連記事

関連記事がまだありません