public override void StateChanged(StateManager.State newState)
{
switch (newState)
{
case StateManager.State.init:
break;
case StateManager.State.positioned:
InfoText.text = "Great!\nNow look straight forward and press your right grip button to calibrate your height.";
RightGripMarker.SetActive(true);
break;
case StateManager.State.calibrated:
InfoText.text = "Place your hands inside the marked areas and press your left grip button to Arm / Disarm. When disarmed, move the Nao with the left Touchpad as shown above.";
StatusText.text = "Calibrated";
StatusText.color = Color.green;
ChangeLayerRecursive(Nao, 0);
LeftModel.SetMeshRendererState(false);
RightModel.SetMeshRendererState(false);
ImageCanvas.SetActive(true);
RightGripMarker.SetActive(false);
//LeftGripMarker.SetActive(true);
RightHandMarker.SetActive(true);
LeftHandMarker.SetActive(true);
FloorMarker.SetActive(false);
break;
case StateManager.State.disarmed:
StatusText.text = "Disarmed";
StatusText.color = new Color(1, 0.5f, 0.2f);
publisher.DoPublish = false;
RightGripMarker.SetActive(false);
//LeftGripMarker.SetActive(true);
RightHandMarker.SetActive(true);
LeftHandMarker.SetActive(true);
break;
case StateManager.State.armed:
InfoCanvas.SetActive(false);
StatusText.text = "Armed";
StatusText.color = Color.red;
publisher.DoPublish = true;
ImageCanvas.SetActive(false);
ChangeLayerRecursive(NaoMirror, 0);
LeftDisplay.SetActive(true);
RightDisplay.SetActive(true);
LeftGripMarker.SetActive(false);
RightHandMarker.SetActive(false);
LeftHandMarker.SetActive(false);
break;
}
}
private void InteractionStream_InteractionFrameReady(object sender, InteractionFrameReadyEventArgs e)
{
using (InteractionFrame interactionFrame = e.OpenInteractionFrame()) //dispose as soon as possible
{
if (interactionFrame == null)
{
return;
}
interactionFrame.CopyInteractionDataTo(userInfos);
}
//select our user and get his data
var userInfo = userInfos.FirstOrDefault(uInfo => uInfo.SkeletonTrackingId != 0);
if (userInfo == null)
{
return;
}
var userID = userInfo.SkeletonTrackingId;
hands = userInfo.HandPointers;
//if at least one hand has been found
if (hands.Count > 0)
{
var mySkeleton = skeletons.FirstOrDefault(skeleton => skeleton.TrackingState == SkeletonTrackingState.Tracked);
var leftHand = hands.FirstOrDefault(hand => hand.HandType == InteractionHandType.Left);
var rightHand = hands.FirstOrDefault(hand => hand.HandType == InteractionHandType.Right);
if (leftHand == null || rightHand == null)
{
return;
}
var leftHandEvent = DetermineLastHandEvent(leftHand, userID);
var rightHandEvent = DetermineLastHandEvent(rightHand, userID);
var leftHandJoint = mySkeleton.Joints[JointType.HandLeft];
var rightHandJoint = mySkeleton.Joints[JointType.HandRight];
LHandText = $"Tracking status: {leftHand.IsTracked}\nX: {Math.Round(leftHand.RawX, 1)} Y: {Math.Round(leftHand.RawY, 1)} Z: {Math.Round(leftHand.RawZ, 1)}\nState: {leftHandEvent}";
RHandText = $"Tracking status: {rightHand.IsTracked}\nX: {Math.Round(rightHand.RawX, 1)} Y: {Math.Round(rightHand.RawY, 1)} Z: {Math.Round(rightHand.RawZ, 1)}\nState: {rightHandEvent}";
LeftHandMarker.UpdateUI(kinectSensor, leftHandJoint, leftHandEvent, leftHandEvent);
NotifyPropertyChanged("LeftHandMarker");
RightHandMarker.UpdateUI(kinectSensor, rightHandJoint, leftHandEvent, rightHandEvent);
NotifyPropertyChanged("RightHandMarker");
if (Manipulator == null || !Manipulator.Connected)
{
return;
}
if (leftHandEvent == InteractionHandEventType.Grip)
{
const int multiplication = 100; //tbd
if (sample)
{
if (rightHandEvent == InteractionHandEventType.Grip && lastGripCommand.Equals(GripOpen))
{
Manipulator?.SendCustom(GripClose);
lastGripCommand = GripClose;
wasGripSent = true;
}
else if (rightHandEvent == InteractionHandEventType.GripRelease && lastGripCommand.Equals(GripClose))
{
Manipulator?.SendCustom(GripOpen);
lastGripCommand = GripOpen;
wasGripSent = true;
}
if (!wasGripSent)
{
var moveCommand = $"DS {(int)sumZ}, {(int)sumX}, {(int)sumY}";
Manipulator?.SendCustom(moveCommand);
}
else
{
wasGripSent = false;
}
timer.Start();
ResetSums();
sample = false;
lastPosition = rightHandJoint.Position;
}
else
{
var deltaX = (rightHandJoint.Position.X - lastPosition.X) * multiplication;
var deltaY = (rightHandJoint.Position.Y - lastPosition.Y) * multiplication;
var deltaZ = (rightHandJoint.Position.Z - lastPosition.Z) * multiplication;
sumX += deltaX;
sumY += deltaY;
sumZ += deltaZ;
}
//lastPosition = rightHandJoint.Position;
}
else
{
lastPosition = rightHandJoint.Position;
}
}
}
