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