private void processSkeletons(kinect.Skeleton[] skeletons)
{
int iSkeleton = 0;
int iSkeletonsTracked = 0;
double panAngle = double.NaN; // degrees from forward; right positive
double tiltAngle = double.NaN;
double targetX = double.NaN; // meters
double targetY = double.NaN;
double targetZ = double.NaN;
kinect.Skeleton trackedSkeleton = (from s in skeletons
where s.TrackingState == kinect.SkeletonTrackingState.Tracked
select s).FirstOrDefault();
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (kinect.Skeleton skel in skeletons)
{
if (kinect.SkeletonTrackingState.Tracked == skel.TrackingState)
{
//RenderClippedEdges(skel, dc);
//this.DrawBonesAndJoints(skel, dc);
//if (gameOnCheckBox.IsChecked.GetValueOrDefault()
// && skel.Joints[kinect.JointType.Head].TrackingState == kinect.JointTrackingState.Tracked
// && skel.Joints[kinect.JointType.HandLeft].TrackingState == kinect.JointTrackingState.Tracked
// && skel.Joints[kinect.JointType.HandRight].TrackingState == kinect.JointTrackingState.Tracked
// && skel.Joints[kinect.JointType.HandLeft].Position.Y > skel.Joints[kinect.JointType.Head].Position.Y
// && skel.Joints[kinect.JointType.HandRight].Position.Y > skel.Joints[kinect.JointType.Head].Position.Y)
//{
// if (!handsUp && (DateTime.Now - lastHandsUp).TotalSeconds > 1.0d)
// {
// speak();
// lastHandsUp = DateTime.Now;
// handsUp = true;
// shootBoth();
// }
// else
// {
// handsUp = false;
// }
//}
// http://social.msdn.microsoft.com/Forums/en-AU/kinectsdk/thread/d821df8d-39ca-44e3-81e7-c907d94acfca - data.UserIndex is always 254
//bool targetedUser = skel.UserIndex == 1;
bool targetedUser = skel == trackedSkeleton;
if (targetedUser)
{
targetX = skel.Position.X; // meters
targetY = skel.Position.Y;
targetZ = skel.Position.Z;
// can set Pan or Tilt to NaN:
panAngle = -Math.Atan2(targetX, targetZ) * 180.0d / Math.PI;
tiltAngle = Math.Atan2(targetY, targetZ) * 180.0d / Math.PI;
}
iSkeletonsTracked++;
}
iSkeleton++;
} // for each skeleton
}
}
if (iSkeletonsTracked > 0)
{
if (!inServoControl)
{
inServoControl = true;
if (!double.IsNaN(panAngle) && !double.IsNaN(tiltAngle))
{
TrajectoryPoint currentPoint = new TrajectoryPoint()
{
X = targetX,
Y = targetY,
Z = targetZ,
panAngle = panAngle,
tiltAngle = tiltAngle
};
double arrowSpeedMSec = 25.0d;
double timeToTargetS = targetZ / arrowSpeedMSec; // +0.01d; // +0.1d; // full shot cycle takes 0.2 sec; arrow will leave the barrel at about 0.1s. High value will lead to a lot of jittering.
TrajectoryPoint futurePoint = predictor.predict(currentPoint, new TimeSpan((long)(timeToTargetS * TimeSpan.TicksPerSecond)));
//double deltaX = futurePoint.X - currentPoint.X;
//double deltaY = futurePoint.Y - currentPoint.Y;
if (gameOnCheckBox.IsChecked.GetValueOrDefault())
{
this.Dispatcher.Invoke(new UpdateLabelDelegate(updatePmValuesLabel), string.Format("X: {0:0.000}\r\nY: {1:0.000}\r\nZ: {2:0.000}\r\nPan: {3:0}\r\nTilt: {4:0}", targetX, targetY, targetZ, panAngle, tiltAngle));
//pololuMaestroConnector.setPanTilt(futurePoint.panAngle, futurePoint.tiltAngle, futurePoint.panAngle, futurePoint.tiltAngle, 0.0d);
// see C:\Projects\Robotics\src\TrackRoamer\TrackRoamerBehaviors\Strategy\StrategyPersonFollowing.cs : 233
double targetPanRelativeToHead = futurePoint.panAngle;
double targetPanRelativeToRobot = measuredKinectPanDegrees + targetPanRelativeToHead;
//Tracer.Trace("================== currentPanKinect=" + _state.currentPanKinect + " targetJoint.Pan=" + targetJoint.Pan + " targetPanRelativeToRobot=" + targetPanRelativeToRobot);
// guns rotate (pan) with Kinect, but tilt independently of Kinect. They are calibrated when Kinect tilt = 0
//targetPan = targetPanRelativeToHead;
//targetTilt = futurePoint.tiltAngle + kinectTiltActualDegrees;
double kinectTurnEstimate = targetPanRelativeToRobot - measuredKinectPanDegrees;
double smallMovementsAngleTreshold = 10.0d;
bool shouldTurnKinect = Math.Abs(kinectTurnEstimate) > smallMovementsAngleTreshold; // don't follow small movements
//kinectPanDesiredDegrees = shouldTurnKinect ? (double?)targetPanRelativeToRobot : null; // will be processed in computeHeadTurn() when head turn measurement comes.
if (shouldTurnKinect)
{
double kinectPanDesiredDegrees = targetPanRelativeToRobot; // will be processed in computeHeadTurn() when head turn measurement comes.
panKinectTargetPosScrollBar.Value = _panTiltAlignment.mksPanKinect(kinectPanDesiredDegrees);
}
}
}
inServoControl = false;
}
}
else if (gameOnCheckBox.IsChecked.GetValueOrDefault())
{
// lost skeletons, center the head:
panKinectTargetPosScrollBar.Value = 1500.0d;
}
}
private void processSkeletons(kinect.Skeleton[] skeletons)
{
int iSkeleton = 0;
int iSkeletonsTracked = 0;
double panAngle = double.NaN; // degrees from forward; right positive
double tiltAngle = double.NaN;
double targetX = double.NaN; // meters
double targetY = double.NaN;
double targetZ = double.NaN;
kinect.Skeleton trackedSkeleton = (from s in skeletons
where s.TrackingState == kinect.SkeletonTrackingState.Tracked
select s).FirstOrDefault();
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (kinect.Skeleton skel in skeletons)
{
if (kinect.SkeletonTrackingState.Tracked == skel.TrackingState)
{
RenderClippedEdges(skel, dc);
this.DrawBonesAndJoints(skel, dc);
// http://social.msdn.microsoft.com/Forums/en-AU/kinectsdk/thread/d821df8d-39ca-44e3-81e7-c907d94acfca - data.UserIndex is always 254
//bool targetedUser = skel.UserIndex == 1;
bool targetedUser = skel == trackedSkeleton;
if (targetedUser)
{
kinect.Joint targetJoint = skel.Joints[kinect.JointType.WristLeft];
bool useTargetJoint = false;
switch (trackWhatComboBox.SelectedValue.ToString())
{
case "HandLeft":
//targetJoint = skel.Joints[kinect.JointType.WristLeft];
useTargetJoint = true;
break;
case "HandRight":
targetJoint = skel.Joints[kinect.JointType.WristRight];
useTargetJoint = true;
break;
case "FootLeft":
targetJoint = skel.Joints[kinect.JointType.FootLeft];
useTargetJoint = true;
break;
case "FootRight":
targetJoint = skel.Joints[kinect.JointType.FootRight];
useTargetJoint = true;
break;
default: // GC - use skel.Position.*
targetX = skel.Position.X; // meters
targetY = skel.Position.Y;
targetZ = skel.Position.Z;
break;
}
if (!useTargetJoint || useTargetJoint && targetJoint.TrackingState == kinect.JointTrackingState.Tracked)
{
if (useTargetJoint)
{
targetX = targetJoint.Position.X;
targetY = targetJoint.Position.Y;
targetZ = targetJoint.Position.Z;
}
// can set Pan or Tilt to NaN:
panAngle = -Math.Atan2(targetX, targetZ) * 180.0d / Math.PI;
tiltAngle = Math.Atan2(targetY, targetZ) * 180.0d / Math.PI;
}
}
iSkeletonsTracked++;
}
else if (skel.TrackingState == kinect.SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
iSkeleton++;
} // for each skeleton
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
if (iSkeletonsTracked > 0)
{
if (!inServoControl)
{
inServoControl = true;
if (!double.IsNaN(panAngle) && !double.IsNaN(tiltAngle))
{
TrajectoryPoint currentPoint = new TrajectoryPoint()
{
X = targetX,
Y = targetY,
Z = targetZ,
panAngle = panAngle,
tiltAngle = tiltAngle
};
double arrowSpeedMSec = 25.0d;
double timeToTargetS = targetZ / arrowSpeedMSec; // +0.01d; // +0.1d; // full shot cycle takes 0.2 sec; arrow will leave the barrel at about 0.1s. High value will lead to a lot of jittering.
TrajectoryPoint futurePoint = predictor.predict(currentPoint, new TimeSpan((long)(timeToTargetS * TimeSpan.TicksPerSecond)));
//double deltaX = futurePoint.X - currentPoint.X;
//double deltaY = futurePoint.Y - currentPoint.Y;
if (enableKinectCheckBox.IsChecked.HasValue && enableKinectCheckBox.IsChecked.Value)
{
this.Dispatcher.Invoke(new UpdateLabelDelegate(updatePmValuesLabel), string.Format("X: {0:0.000}\r\nY: {1:0.000}\r\nZ: {2:0.000}\r\nPan: {3:0}\r\nTilt: {4:0}", targetX, targetY, targetZ, panAngle, tiltAngle));
pololuMaestroConnector.setPanTilt(futurePoint.panAngle, futurePoint.tiltAngle, futurePoint.panAngle, futurePoint.tiltAngle, 0.0d);
}
}
inServoControl = false;
}
}
else
{
if ((DateTime.Now - lastNotTracking).TotalSeconds > 10.0d)
{
//speak("come out, come out, wherever you are!");
lastNotTracking = DateTime.Now;
if (enableKinectCheckBox.IsChecked.HasValue && enableKinectCheckBox.IsChecked.Value)
{
SafePosture();
}
}
isTracking = false;
}
}
