Azure Kinect BodyTracking SDK を Unity から TCP 経由で Node-RED にデータを送るメモ
Azure Kinect BodyTracking SDK を Unity から TCP 経由で Node-RED にデータを送るメモです。
経緯
https://www.1ft-seabass.jp/memo/2020/03/31/azure-kinect-dk-firststep/
このあたりや、
https://www.1ft-seabass.jp/memo/2020/07/15/cool-batfile-azure-kinect-bodytracking-for-unity/
このあたりを経て、Azure Kinect BodyTracking SDK を Unity で動かせるようになりました。
https://twitter.com/1ft_seabass/status/1296723062235131904
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"]]}]実行してみる
Unity の Play ボタンをクリックしてみると、
今までと同じように、ボーンが同期しているの同じです。
うまく動いていれば、このようにコンソールに表示されます。
Node-REDでは接続数が1つ増えて、
このように、すごい頻度でデータが来ます。
SpineNavelのように部位名にしてオブジェクトを作っていますが、ちゃんと設定されています。