Azure Kinect BodyTracking SDK を Unity から TCP 経由で Node-RED にデータを送るメモ

Azure Kinect BodyTracking SDK を Unity から TCP 経由で Node-RED にデータを送るメモです。

経緯

Azure Kinect DKをひとまず動かしたメモ

このあたりや、

Azure Kinect BodyTracking SDK の Unity セットアップで付属のライブラリ移動バッチが便利だったメモ

このあたりを経て、Azure Kinect BodyTracking SDK を Unity で動かせるようになりました。

よし！ Node-RED で TCP サーバー立てて、 Azure Kinect DK の Body Tracking SDK から JSON でボーンデータで流せた！#nodered #noderedjp #AzureKinect #Unity pic.twitter.com/zyOAtC6E7C

— Tanaka Seigo (@1ft_seabass) August 21, 2020

Unity のコードを修正

こちらの GitHub の body-tracking-samples の sample_unity_bodytracking Unityサンプルを使っています。

おおむね、 Azure Kinect BodyTracking SDK の Unity セットアップで付属のライブラリ移動バッチが便利だったメモの流れを踏襲しています。

実際にKinectからデータを受けて、Unityの中の人形にデータを送っている、 sample_unity_bodytracking/Assets/Scripts/TrackerHandler.cs を以下のように修正しました。

JSON周りは MiniJSON を使っています。

using System.Collections.Generic;
using UnityEngine;
using Microsoft.Azure.Kinect.BodyTracking;
using System.IO;
using System;
using MiniJSON;

using System.Net.Sockets;
using System.Text;

public class TrackerHandler : MonoBehaviour
{
    public Dictionary<JointId, JointId> parentJointMap;
    Dictionary<JointId, Quaternion> basisJointMap;
    public Quaternion[] absoluteJointRotations = new Quaternion[(int)JointId.Count];
    public bool drawSkeletons = true;
    Quaternion Y_180_FLIP = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);

    // TCP処理
    TcpClient client;
    NetworkStream stream;
    // IDから部位名を割り出すためのDictionary
    public Dictionary<JointId, string> listJointName;

    // Start is called before the first frame update
    void Awake()
    {
        // TCP準備 //////////////////////////////////////

        // ローカル 127.0.0.1 のNode-RED TCPポート 18801（自分で決めたもの）
        // もちろん、外部のサーバーに設置した Node-RED でもOK。自分の設置場所に応じて変える。
        client = new TcpClient("127.0.0.1", 18801);
        stream = client.GetStream();

        // 読み取り・書き込みのタイムアウトを指定する
        stream.ReadTimeout = 10000;
        stream.WriteTimeout = 10000;

        parentJointMap = new Dictionary<JointId, JointId>();

        // pelvis has no parent so set to count
        parentJointMap[JointId.Pelvis] = JointId.Count;
        parentJointMap[JointId.SpineNavel] = JointId.Pelvis;
        parentJointMap[JointId.SpineChest] = JointId.SpineNavel;
        parentJointMap[JointId.Neck] = JointId.SpineChest;
        parentJointMap[JointId.ClavicleLeft] = JointId.SpineChest;
        parentJointMap[JointId.ShoulderLeft] = JointId.ClavicleLeft;
        parentJointMap[JointId.ElbowLeft] = JointId.ShoulderLeft;
        parentJointMap[JointId.WristLeft] = JointId.ElbowLeft;
        parentJointMap[JointId.HandLeft] = JointId.WristLeft;
        parentJointMap[JointId.HandTipLeft] = JointId.HandLeft;
        parentJointMap[JointId.ThumbLeft] = JointId.HandLeft;
        parentJointMap[JointId.ClavicleRight] = JointId.SpineChest;
        parentJointMap[JointId.ShoulderRight] = JointId.ClavicleRight;
        parentJointMap[JointId.ElbowRight] = JointId.ShoulderRight;
        parentJointMap[JointId.WristRight] = JointId.ElbowRight;
        parentJointMap[JointId.HandRight] = JointId.WristRight;
        parentJointMap[JointId.HandTipRight] = JointId.HandRight;
        parentJointMap[JointId.ThumbRight] = JointId.HandRight;
        parentJointMap[JointId.HipLeft] = JointId.SpineNavel;
        parentJointMap[JointId.KneeLeft] = JointId.HipLeft;
        parentJointMap[JointId.AnkleLeft] = JointId.KneeLeft;
        parentJointMap[JointId.FootLeft] = JointId.AnkleLeft;
        parentJointMap[JointId.HipRight] = JointId.SpineNavel;
        parentJointMap[JointId.KneeRight] = JointId.HipRight;
        parentJointMap[JointId.AnkleRight] = JointId.KneeRight;
        parentJointMap[JointId.FootRight] = JointId.AnkleRight;
        parentJointMap[JointId.Head] = JointId.Pelvis;
        parentJointMap[JointId.Nose] = JointId.Head;
        parentJointMap[JointId.EyeLeft] = JointId.Head;
        parentJointMap[JointId.EarLeft] = JointId.Head;
        parentJointMap[JointId.EyeRight] = JointId.Head;
        parentJointMap[JointId.EarRight] = JointId.Head;

        // IDから部位名を割り出すためのDictionary
        listJointName = new Dictionary<JointId, string>();
        listJointName[JointId.Pelvis] = "Pelvis";
        listJointName[JointId.SpineNavel] = "SpineNavel";
        listJointName[JointId.SpineChest] = "SpineChest";
        listJointName[JointId.Neck] = "Neck";
        listJointName[JointId.ClavicleLeft] = "ClavicleLeft";
        listJointName[JointId.ShoulderLeft] = "ShoulderLeft";
        listJointName[JointId.ElbowLeft] = "ElbowLeft";
        listJointName[JointId.WristLeft] = "WristLeft";
        listJointName[JointId.HandLeft] = "HandLeft";
        listJointName[JointId.HandTipLeft] = "HandTipLeft";
        listJointName[JointId.ThumbLeft] = "ThumbLeft";
        listJointName[JointId.ClavicleRight] = "ClavicleRight";
        listJointName[JointId.ShoulderRight] = "ShoulderRight";
        listJointName[JointId.ElbowRight] = "ElbowRight";
        listJointName[JointId.WristRight] = "WristRight";
        listJointName[JointId.HandRight] = "HandRight";
        listJointName[JointId.HandTipRight] = "HandTipRight";
        listJointName[JointId.ThumbRight] = "ThumbRight";
        listJointName[JointId.HipLeft] = "HipLeft";
        listJointName[JointId.KneeLeft] = "KneeLeft";
        listJointName[JointId.AnkleLeft] = "AnkleLeft";
        listJointName[JointId.FootLeft] = "FootLeft";
        listJointName[JointId.HipRight] = "HipRight";
        listJointName[JointId.KneeRight] = "KneeRight";
        listJointName[JointId.AnkleRight] = "AnkleRight";
        listJointName[JointId.FootRight] = "FootRight";
        listJointName[JointId.Head] = "Head";
        listJointName[JointId.Nose] = "Nose";
        listJointName[JointId.EyeLeft] = "EyeLeft";
        listJointName[JointId.EarLeft] = "EarLeft";
        listJointName[JointId.EyeRight] = "EyeRight";
        listJointName[JointId.EarRight] = "EarRight";

        Vector3 zpositive = Vector3.forward;
        Vector3 xpositive = Vector3.right;
        Vector3 ypositive = Vector3.up;
        // spine and left hip are the same
        Quaternion leftHipBasis = Quaternion.LookRotation(xpositive, -zpositive);
        Quaternion spineHipBasis = Quaternion.LookRotation(xpositive, -zpositive);
        Quaternion rightHipBasis = Quaternion.LookRotation(xpositive, zpositive);
        // arms and thumbs share the same basis
        Quaternion leftArmBasis = Quaternion.LookRotation(ypositive, -zpositive);
        Quaternion rightArmBasis = Quaternion.LookRotation(-ypositive, zpositive);
        Quaternion leftHandBasis = Quaternion.LookRotation(-zpositive, -ypositive);
        Quaternion rightHandBasis = Quaternion.identity;
        Quaternion leftFootBasis = Quaternion.LookRotation(xpositive, ypositive);
        Quaternion rightFootBasis = Quaternion.LookRotation(xpositive, -ypositive);

        basisJointMap = new Dictionary<JointId, Quaternion>();

        // pelvis has no parent so set to count
        basisJointMap[JointId.Pelvis] = spineHipBasis;
        basisJointMap[JointId.SpineNavel] = spineHipBasis;
        basisJointMap[JointId.SpineChest] = spineHipBasis;
        basisJointMap[JointId.Neck] = spineHipBasis;
        basisJointMap[JointId.ClavicleLeft] = leftArmBasis;
        basisJointMap[JointId.ShoulderLeft] = leftArmBasis;
        basisJointMap[JointId.ElbowLeft] = leftArmBasis;
        basisJointMap[JointId.WristLeft] = leftHandBasis;
        basisJointMap[JointId.HandLeft] = leftHandBasis;
        basisJointMap[JointId.HandTipLeft] = leftHandBasis;
        basisJointMap[JointId.ThumbLeft] = leftArmBasis;
        basisJointMap[JointId.ClavicleRight] = rightArmBasis;
        basisJointMap[JointId.ShoulderRight] = rightArmBasis;
        basisJointMap[JointId.ElbowRight] = rightArmBasis;
        basisJointMap[JointId.WristRight] = rightHandBasis;
        basisJointMap[JointId.HandRight] = rightHandBasis;
        basisJointMap[JointId.HandTipRight] = rightHandBasis;
        basisJointMap[JointId.ThumbRight] = rightArmBasis;
        basisJointMap[JointId.HipLeft] = leftHipBasis;
        basisJointMap[JointId.KneeLeft] = leftHipBasis;
        basisJointMap[JointId.AnkleLeft] = leftHipBasis;
        basisJointMap[JointId.FootLeft] = leftFootBasis;
        basisJointMap[JointId.HipRight] = rightHipBasis;
        basisJointMap[JointId.KneeRight] = rightHipBasis;
        basisJointMap[JointId.AnkleRight] = rightHipBasis;
        basisJointMap[JointId.FootRight] = rightFootBasis;
        basisJointMap[JointId.Head] = spineHipBasis;
        basisJointMap[JointId.Nose] = spineHipBasis;
        basisJointMap[JointId.EyeLeft] = spineHipBasis;
        basisJointMap[JointId.EarLeft] = spineHipBasis;
        basisJointMap[JointId.EyeRight] = spineHipBasis;
        basisJointMap[JointId.EarRight] = spineHipBasis;
    }

    public void updateTracker(BackgroundData trackerFrameData)
    {
        //this is an array in case you want to get the n closest bodies
        int closestBody = findClosestTrackedBody(trackerFrameData);

        // render the closest body
        Body skeleton = trackerFrameData.Bodies[closestBody];
        renderSkeleton(skeleton, 0);
    }

    int findIndexFromId(BackgroundData frameData, int id)
    {
        int retIndex = -1;
        for (int i = 0; i < (int)frameData.NumOfBodies; i++)
        {
            if ((int)frameData.Bodies[i].Id == id)
            {
                retIndex = i;
                break;
            }
        }
        return retIndex;
    }

    private int findClosestTrackedBody(BackgroundData trackerFrameData)
    {
        int closestBody = -1;
        const float MAX_DISTANCE = 5000.0f;
        float minDistanceFromKinect = MAX_DISTANCE;
        for (int i = 0; i < (int)trackerFrameData.NumOfBodies; i++)
        {
            var pelvisPosition = trackerFrameData.Bodies[i].JointPositions3D[(int)JointId.Pelvis];
            Vector3 pelvisPos = new Vector3((float)pelvisPosition.X, (float)pelvisPosition.Y, (float)pelvisPosition.Z);
            if (pelvisPos.magnitude < minDistanceFromKinect)
            {
                closestBody = i;
                minDistanceFromKinect = pelvisPos.magnitude;
            }
        }
        return closestBody;
    }

    public void turnOnOffSkeletons()
    {
        drawSkeletons = !drawSkeletons;
        const int bodyRenderedNum = 0;
        for (int jointNum = 0; jointNum < (int)JointId.Count; jointNum++)
        {
            transform.GetChild(bodyRenderedNum).GetChild(jointNum).gameObject.GetComponent<MeshRenderer>().enabled = drawSkeletons;
            transform.GetChild(bodyRenderedNum).GetChild(jointNum).GetChild(0).GetComponent<MeshRenderer>().enabled = drawSkeletons;
        }
    }


    public void renderSkeleton(Body skeleton, int skeletonNumber)
    {
        Dictionary<string, object> jointDatas = new Dictionary<string, object>();

        // 処理前のタイムスタンプ
        jointDatas["timestampStart"] = DateTime.Now.ToString() + "." + DateTime.Now.Millisecond;

        // 部位ぶんループする
        for (int jointNum = 0; jointNum < (int)JointId.Count; jointNum++)
        {

            // Kinectから来た素のJointデータをこの部位 _jointInfo で記録
            Dictionary<string, object> _jointInfo= new Dictionary<string, object>();

            float _posX = skeleton.JointPositions3D[jointNum].X;
            float _posY = skeleton.JointPositions3D[jointNum].X;
            float _posZ = skeleton.JointPositions3D[jointNum].Z;

            float _rotX = skeleton.JointRotations[jointNum].X;
            float _rotY = skeleton.JointRotations[jointNum].Y;
            float _rotZ = skeleton.JointRotations[jointNum].Z;

            _jointInfo["posX"] = _posX;
            _jointInfo["posY"] = _posY;
            _jointInfo["posZ"] = _posZ;
            _jointInfo["rotX"] = _rotX;
            _jointInfo["rotY"] = _rotY;
            _jointInfo["rotZ"] = _rotZ;


            Vector3 jointPos = new Vector3(skeleton.JointPositions3D[jointNum].X, -skeleton.JointPositions3D[jointNum].Y, skeleton.JointPositions3D[jointNum].Z);
            Vector3 offsetPosition = transform.rotation * jointPos;
            Vector3 positionInTrackerRootSpace = transform.position + offsetPosition;
            Quaternion jointRot = Y_180_FLIP * new Quaternion(skeleton.JointRotations[jointNum].X, skeleton.JointRotations[jointNum].Y,
                skeleton.JointRotations[jointNum].Z, skeleton.JointRotations[jointNum].W) * Quaternion.Inverse(basisJointMap[(JointId)jointNum]);
            absoluteJointRotations[jointNum] = jointRot;
            // these are absolute body space because each joint has the body root for a parent in the scene graph
            transform.GetChild(skeletonNumber).GetChild(jointNum).localPosition = jointPos;
            transform.GetChild(skeletonNumber).GetChild(jointNum).localRotation = jointRot;

            // この部位を jointNum ジョイント番号から取得
            string jointName = listJointName[(JointId)jointNum];

            // この部位の情報 _jointInfo を jointDatas にぶら下げる
            jointDatas[jointName] = _jointInfo;


            const int boneChildNum = 0;
            if (parentJointMap[(JointId)jointNum] != JointId.Head && parentJointMap[(JointId)jointNum] != JointId.Count)
            {
                Vector3 parentTrackerSpacePosition = new Vector3(skeleton.JointPositions3D[(int)parentJointMap[(JointId)jointNum]].X,
                    -skeleton.JointPositions3D[(int)parentJointMap[(JointId)jointNum]].Y, skeleton.JointPositions3D[(int)parentJointMap[(JointId)jointNum]].Z);
                Vector3 boneDirectionTrackerSpace = jointPos - parentTrackerSpacePosition;
                Vector3 boneDirectionWorldSpace = transform.rotation * boneDirectionTrackerSpace;
                Vector3 boneDirectionLocalSpace = Quaternion.Inverse(transform.GetChild(skeletonNumber).GetChild(jointNum).rotation) * Vector3.Normalize(boneDirectionWorldSpace);
                transform.GetChild(skeletonNumber).GetChild(jointNum).GetChild(boneChildNum).localScale = new Vector3(1, 20.0f * 0.5f * boneDirectionWorldSpace.magnitude, 1);
                transform.GetChild(skeletonNumber).GetChild(jointNum).GetChild(boneChildNum).localRotation = Quaternion.FromToRotation(Vector3.up, boneDirectionLocalSpace);
                transform.GetChild(skeletonNumber).GetChild(jointNum).GetChild(boneChildNum).position = transform.GetChild(skeletonNumber).GetChild(jointNum).position - 0.5f * boneDirectionWorldSpace;
            }
            else
            {
                transform.GetChild(skeletonNumber).GetChild(jointNum).GetChild(boneChildNum).gameObject.SetActive(false);
            }

        }

        // TCP処理　///////////////////////

        // 処理後のタイムスタンプ
        jointDatas["timestampFinish"] = DateTime.Now.ToString() + "." + DateTime.Now.Millisecond;

        // シリアライズしてJSONの文字列にする
        string jsonDataString = Json.Serialize(jointDatas);
        
        string sendMsg = jsonDataString;

        Debug.Log("TCP送信");

        // サーバーにデータを送信する文字列をByte型配列に変換
        var sendBytes = Encoding.UTF8.GetBytes(sendMsg + '\n');

        // データを送信する
        stream.Write(sendBytes, 0, sendBytes.Length);
        
    }

    public Quaternion GetRelativeJointRotation(JointId jointId)
    {
        JointId parent = parentJointMap[jointId];
        Quaternion parentJointRotationBodySpace = Quaternion.identity;
        if (parent == JointId.Count)
        {
            parentJointRotationBodySpace = Y_180_FLIP;
        }
        else
        {
            parentJointRotationBodySpace = absoluteJointRotations[(int)parent];
        }
        Quaternion jointRotationBodySpace = absoluteJointRotations[(int)jointId];
        Quaternion relativeRotation =  Quaternion.Inverse(parentJointRotationBodySpace) * jointRotationBodySpace;

        return relativeRotation;
    }

    void OnDestroy()
    {
        // 終了時に止める
        stream.Close();
        client.Close();
    }

}

Unity TCP通信については、Unity　TCP通信を受信する – はかせのラボがとても役に立ちました。ありがとうございます！

Node-RED のフロー

データ受け付ける Node-RED のフローはこちらです。

TCPのノードでは以下のように設定しています。

ポートは 18801 にしていて、出力は ストリーム の 文字列 に設定しています。

のこりの2つは、もらった文字列を JSON データ化して、デバッグタブに結果を出すものです。

こちらのフローをそのままインポートして使える JSON データも置いておきます。

[{"id":"659ab563.e651dc","type":"tcp in","z":"6c99e2e9.8ea0ac","name":"","server":"server","host":"","port":"18801","datamode":"stream","datatype":"utf8","newline":"","topic":"","base64":false,"x":300,"y":240,"wires":[["3ba4f98d.754846"]]},{"id":"f95d1f55.f6774","type":"debug","z":"6c99e2e9.8ea0ac","name":"","active":true,"tosidebar":true,"console":false,"tostatus":false,"complete":"false","statusVal":"","statusType":"auto","x":650,"y":240,"wires":[]},{"id":"3ba4f98d.754846","type":"json","z":"6c99e2e9.8ea0ac","name":"","property":"payload","action":"","pretty":false,"x":470,"y":240,"wires":[["f95d1f55.f6774"]]}]