async void InitCurrentPoseListTopic()
{
tokenCurrentPoseList = await ubiiNode.SubscribeTopic(ubiiComponentAvatarCurrentPose.GetTopicCurrentPoseList(), (Ubii.TopicData.TopicDataRecord record) =>
{
Google.Protobuf.Collections.RepeatedField <Ubii.DataStructure.Object3D> objects = record.Object3DList.Elements;
for (int i = 0; i < record.Object3DList.Elements.Count; i++)
{
string boneString = record.Object3DList.Elements[i].Id;
HumanBodyBones bone;
if (HumanBodyBones.TryParse(boneString, out bone))
{
Ubii.DataStructure.Pose3D pose = record.Object3DList.Elements[i].Pose;
UbiiPose3D newMapPose = new UbiiPose3D
{
position = new Vector3((float)pose.Position.X, (float)pose.Position.Y, (float)pose.Position.Z),
rotation = new Quaternion((float)pose.Quaternion.X, (float)pose.Quaternion.Y, (float)pose.Quaternion.Z, (float)pose.Quaternion.W)
};
if (mapBone2CurrentPose.ContainsKey(bone))
{
mapBone2CurrentPose[bone] = newMapPose;
}
else
{
mapBone2CurrentPose.Add(bone, newMapPose);
}
}
}
});
}
async void OnUbiiNodeInitialized()
{
ubiiSpecs = new Ubii.Devices.Component
{
Name = NAME,
Description = DESCRIPTION,
MessageFormat = MESSAGE_FORMAT,
IoType = Ubii.Devices.Component.Types.IOType.Subscriber,
Topic = GetTopicTargetVelocities()
};
ubiiSpecs.Tags.AddRange(TAGS);
tokenTargetVelocities = await ubiiNode.SubscribeTopic(this.ubiiSpecs.Topic, (Ubii.TopicData.TopicDataRecord record) =>
{
for (int i = 0; i < record.Object3DList.Elements.Count; i++)
{
string boneString = record.Object3DList.Elements[i].Id;
HumanBodyBones bone;
if (HumanBodyBones.TryParse(boneString, out bone))
{
Ubii.DataStructure.Pose3D pose = record.Object3DList.Elements[i].Pose;
Vector3 linear = new Vector3((float)pose.Position.X, (float)pose.Position.Y, (float)pose.Position.Z);
Vector3 angular = new Vector3((float)pose.Euler.X, (float)pose.Euler.Y, (float)pose.Euler.Z);
SetTargetVelocity(bone, linear, angular);
}
}
});
ubiiReady = true;
}
async void InitIKTopics()
{
foreach (IK_TARGET ikTarget in Enum.GetValues(typeof(IK_TARGET)))
{
GameObject ikTargetObject = new GameObject("IK-Target PoseEstimator " + ikTarget.ToString());
mapIKTargetTransforms.Add(ikTarget, ikTargetObject.transform);
mapIKTarget2UbiiPose.Add(ikTarget, new UbiiPose3D
{
position = new Vector3(),
rotation = new Quaternion()
});
}
tokenIkTargetPose = await ubiiNode.SubscribeTopic(ubiiComponentIkTargets.GetTopicIKTargets(), (Ubii.TopicData.TopicDataRecord record) =>
{
for (int i = 0; i < record.Object3DList.Elements.Count; i++)
{
string ikTargetString = record.Object3DList.Elements[i].Id;
Ubii.DataStructure.Pose3D pose3D = record.Object3DList.Elements[i].Pose;
IK_TARGET ikTarget;
if (IK_TARGET.TryParse(ikTargetString, out ikTarget))
{
UbiiPose3D pose = mapIKTarget2UbiiPose[ikTarget];
pose.position.Set(
(float)pose3D.Position.X,
(float)pose3D.Position.Y,
(float)pose3D.Position.Z);
pose.rotation.Set(
(float)pose3D.Quaternion.X,
(float)pose3D.Quaternion.Y,
(float)pose3D.Quaternion.Z,
(float)pose3D.Quaternion.W);
mapIKTarget2UbiiPose[ikTarget] = pose;
}
}
});
initialized = true;
}
