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

Hirayama 2023/04/21

Rust
mocopi

はじめに

今年の 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. - seagetch/mcp-receiver

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

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

調査

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

mocopi からデータを受信する

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

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

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

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

mocopiデータをパース

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

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

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);
        }
    }
}

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