public VisualizableJoint(kinect.JointType jt)
{
this.TrackingState = kinect.JointTrackingState.NotTracked;
this.jointType = jt;
this.IsJointOfInterest = false;
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, kinect.SkeletonFrameReadyEventArgs e)
{
kinect.Skeleton[] skeletons = new kinect.Skeleton[0];
using (kinect.SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new kinect.Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
processSkeletons(skeletons);
}
/// <summary>
/// fills this.SkeletonPose - may leave it as NotDetected or fill None
/// </summary>
/// <param name="skel">Skeleton</param>
public void detectSkeletonPose(kinect.Skeleton skel)
{
// We come here only when skeleton and its Head is tracked. For pose detection spine and both wrists must be tracked too.
if (skel.Joints[kinect.JointType.WristLeft].TrackingState == kinect.JointTrackingState.Tracked
&& skel.Joints[kinect.JointType.WristRight].TrackingState == kinect.JointTrackingState.Tracked
&& skel.Joints[kinect.JointType.Spine].TrackingState == kinect.JointTrackingState.Tracked)
{
double sizeFactor = SkeletonSizeMeters.HasValue ? SkeletonSizeMeters.Value / 1.7d : 1.0d; // account for persons with height different from my 1.7 meters
// coordinates in meters:
//Tracer.Trace("Hands: " + skel.Joints[kinect.JointType.WristLeft].Position.X + " " + skel.Joints[kinect.JointType.WristRight].Position.X
// + " - " + skel.Joints[kinect.JointType.WristLeft].Position.Y + " " + skel.Joints[kinect.JointType.WristRight].Position.Y
// + " - " + skel.Joints[kinect.JointType.WristLeft].Position.Z + " " + skel.Joints[kinect.JointType.WristRight].Position.Z);
bool leftHandUp = skel.Joints[kinect.JointType.WristLeft].Position.Y > skel.Joints[kinect.JointType.Head].Position.Y;
bool rightHandUp = skel.Joints[kinect.JointType.WristRight].Position.Y > skel.Joints[kinect.JointType.Head].Position.Y;
if (leftHandUp && rightHandUp)
{
this.SkeletonPose = SkeletonPose.HandsUp;
}
else if (leftHandUp)
{
this.SkeletonPose = SkeletonPose.LeftHandUp;
}
else if (rightHandUp)
{
this.SkeletonPose = SkeletonPose.RightHandUp;
}
else
{
bool leftHandForward = skel.Joints[kinect.JointType.WristLeft].Position.Z < skel.Joints[kinect.JointType.Spine].Position.Z - 0.4f * sizeFactor;
bool leftHandToTheSide = skel.Joints[kinect.JointType.WristLeft].Position.X < skel.Joints[kinect.JointType.Spine].Position.X - 0.6f * sizeFactor; // meters
bool leftHandPointingRight = skel.Joints[kinect.JointType.WristLeft].Position.X > skel.Joints[kinect.JointType.Spine].Position.X + 0.2f * sizeFactor; // meters
bool rightHandForward = skel.Joints[kinect.JointType.WristRight].Position.Z < skel.Joints[kinect.JointType.Spine].Position.Z - 0.4f * sizeFactor;
bool rightHandToTheSide = skel.Joints[kinect.JointType.WristRight].Position.X > skel.Joints[kinect.JointType.Spine].Position.X + 0.6f * sizeFactor;
bool rightHandPointingLeft = skel.Joints[kinect.JointType.WristRight].Position.X < skel.Joints[kinect.JointType.Spine].Position.X - 0.2f * sizeFactor;
bool HandsCrossed = skel.Joints[kinect.JointType.WristRight].Position.X < skel.Joints[kinect.JointType.WristLeft].Position.X - 0.2f * sizeFactor; // criteria looser than leftHandPointingRight && rightHandPointingLeft
if (leftHandForward && rightHandForward)
{
this.SkeletonPose = SkeletonPose.BothArmsForward;
}
else if (leftHandForward)
{
this.SkeletonPose = SkeletonPose.LeftArmForward;
}
else if (rightHandForward)
{
this.SkeletonPose = SkeletonPose.RightArmForward;
}
else if (leftHandToTheSide && rightHandToTheSide)
{
this.SkeletonPose = SkeletonPose.BothArmsOut;
}
else if (leftHandToTheSide)
{
this.SkeletonPose = SkeletonPose.LeftArmOut;
}
else if (rightHandToTheSide)
{
this.SkeletonPose = SkeletonPose.RightArmOut;
}
else if (HandsCrossed)
{
this.SkeletonPose = SkeletonPose.ArmsCrossed;
}
else if (leftHandPointingRight)
{
this.SkeletonPose = SkeletonPose.LeftHandPointingRight;
}
else if (rightHandPointingLeft)
{
this.SkeletonPose = SkeletonPose.RightHandPointingLeft;
}
else
{
this.SkeletonPose = SkeletonPose.None;
}
}
}
}
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;
}
}
void SensorColorFrameReady(object sender, kinect.ColorImageFrameReadyEventArgs e)
{
using (kinect.ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (colorFrame != null)
{
// Copy the pixel data from the image to a temporary array
colorFrame.CopyPixelDataTo(this.colorFramePixels);
for (int i = 3; i < this.colorFramePixels.Length - 4; i += 4)
{
this.colorFramePixels[i] = 255; // set the alpha to max
}
// Write the pixel data into our bitmap
this.colorBitmapVideo.WritePixels(
new Int32Rect(0, 0, this.colorBitmapVideo.PixelWidth, this.colorBitmapVideo.PixelHeight),
this.colorFramePixels,
this.colorBitmapVideo.PixelWidth * sizeof(int),
0);
}
}
// draw a sighting frame for precise alignment:
// Horisontally the frame spans 16.56 degrees on every side and 12.75 degrees either up or down (at 74" the size it covers is 44"W by 33.5"H, i.e. 33.12 degrees by 25.5 degrees)
// see http://writeablebitmapex.codeplex.com/
//int sightFrameColor = 255; // Pbgra32
Color sightFrameColor = Colors.Red;
colorBitmapVideo.DrawLine((int)colorBitmapVideo.Width / 2, 0, (int)colorBitmapVideo.Width / 2, (int)colorBitmapVideo.Height, sightFrameColor);
colorBitmapVideo.DrawLine(0, (int)colorBitmapVideo.Height / 2, (int)colorBitmapVideo.Width, (int)colorBitmapVideo.Height / 2, sightFrameColor);
colorBitmapVideo.DrawRectangle((int)colorBitmapVideo.Width / 4, (int)colorBitmapVideo.Height / 4,
(int)colorBitmapVideo.Width * 3 / 4, (int)colorBitmapVideo.Height * 3 / 4, sightFrameColor);
}
/// <summary>
/// Draws a skeleton's bones and joints
/// </summary>
/// <param name="skeleton">skeleton to draw</param>
/// <param name="drawingContext">drawing context to draw to</param>
private void DrawBonesAndJoints(kinect.Skeleton skeleton, DrawingContext drawingContext)
{
// Render Torso
this.DrawBone(skeleton, drawingContext, kinect.JointType.Head, kinect.JointType.ShoulderCenter);
this.DrawBone(skeleton, drawingContext, kinect.JointType.ShoulderCenter, kinect.JointType.ShoulderLeft);
this.DrawBone(skeleton, drawingContext, kinect.JointType.ShoulderCenter, kinect.JointType.ShoulderRight);
this.DrawBone(skeleton, drawingContext, kinect.JointType.ShoulderCenter, kinect.JointType.Spine);
this.DrawBone(skeleton, drawingContext, kinect.JointType.Spine, kinect.JointType.HipCenter);
this.DrawBone(skeleton, drawingContext, kinect.JointType.HipCenter, kinect.JointType.HipLeft);
this.DrawBone(skeleton, drawingContext, kinect.JointType.HipCenter, kinect.JointType.HipRight);
// Left Arm
this.DrawBone(skeleton, drawingContext, kinect.JointType.ShoulderLeft, kinect.JointType.ElbowLeft);
this.DrawBone(skeleton, drawingContext, kinect.JointType.ElbowLeft, kinect.JointType.WristLeft);
this.DrawBone(skeleton, drawingContext, kinect.JointType.WristLeft, kinect.JointType.HandLeft);
// Right Arm
this.DrawBone(skeleton, drawingContext, kinect.JointType.ShoulderRight, kinect.JointType.ElbowRight);
this.DrawBone(skeleton, drawingContext, kinect.JointType.ElbowRight, kinect.JointType.WristRight);
this.DrawBone(skeleton, drawingContext, kinect.JointType.WristRight, kinect.JointType.HandRight);
// Left Leg
this.DrawBone(skeleton, drawingContext, kinect.JointType.HipLeft, kinect.JointType.KneeLeft);
this.DrawBone(skeleton, drawingContext, kinect.JointType.KneeLeft, kinect.JointType.AnkleLeft);
this.DrawBone(skeleton, drawingContext, kinect.JointType.AnkleLeft, kinect.JointType.FootLeft);
// Right Leg
this.DrawBone(skeleton, drawingContext, kinect.JointType.HipRight, kinect.JointType.KneeRight);
this.DrawBone(skeleton, drawingContext, kinect.JointType.KneeRight, kinect.JointType.AnkleRight);
this.DrawBone(skeleton, drawingContext, kinect.JointType.AnkleRight, kinect.JointType.FootRight);
// Render Joints
foreach (kinect.Joint joint in skeleton.Joints)
{
Brush drawBrush = null;
if (joint.TrackingState == kinect.JointTrackingState.Tracked)
{
drawBrush = this.trackedJointBrush;
}
else if (joint.TrackingState == kinect.JointTrackingState.Inferred)
{
drawBrush = this.inferredJointBrush;
}
if (drawBrush != null)
{
drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
}
}
}
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;
}
}
/// <summary>
/// Draws a bone line between two joints
/// </summary>
/// <param name="skeleton">skeleton to draw bones from</param>
/// <param name="drawingContext">drawing context to draw to</param>
/// <param name="jointType0">joint to start drawing from</param>
/// <param name="jointType1">joint to end drawing at</param>
private void DrawBone(kinect.Skeleton skeleton, DrawingContext drawingContext, kinect.JointType jointType0, kinect.JointType jointType1)
{
kinect.Joint joint0 = skeleton.Joints[jointType0];
kinect.Joint joint1 = skeleton.Joints[jointType1];
// If we can't find either of these joints, exit
if (joint0.TrackingState == kinect.JointTrackingState.NotTracked ||
joint1.TrackingState == kinect.JointTrackingState.NotTracked)
{
return;
}
// Don't draw if both points are inferred
if (joint0.TrackingState == kinect.JointTrackingState.Inferred &&
joint1.TrackingState == kinect.JointTrackingState.Inferred)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = this.inferredBonePen;
if (joint0.TrackingState == kinect.JointTrackingState.Tracked && joint1.TrackingState == kinect.JointTrackingState.Tracked)
{
drawPen = this.trackedBonePen;
}
drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
}
/// <summary>
/// Draws indicators to show which edges are clipping skeleton data
/// </summary>
/// <param name="skeleton">skeleton to draw clipping information for</param>
/// <param name="drawingContext">drawing context to draw to</param>
private static void RenderClippedEdges(kinect.Skeleton skeleton, DrawingContext drawingContext)
{
if (skeleton.ClippedEdges.HasFlag(kinect.FrameEdges.Bottom))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, RenderHeight - ClipBoundsThickness, RenderWidth, ClipBoundsThickness));
}
if (skeleton.ClippedEdges.HasFlag(kinect.FrameEdges.Top))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, RenderWidth, ClipBoundsThickness));
}
if (skeleton.ClippedEdges.HasFlag(kinect.FrameEdges.Left))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, ClipBoundsThickness, RenderHeight));
}
if (skeleton.ClippedEdges.HasFlag(kinect.FrameEdges.Right))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(RenderWidth - ClipBoundsThickness, 0, ClipBoundsThickness, RenderHeight));
}
}
/// <summary>
/// Maps a SkeletonPoint to lie within our render space and converts to Point
/// </summary>
/// <param name="skelpoint">point to map</param>
/// <returns>mapped point</returns>
private Point SkeletonPointToScreen(kinect.SkeletonPoint skelpoint)
{
// Convert point to depth space.
// We are not using depth directly, but we do want the points in our 640x480 output resolution.
kinect.DepthImagePoint depthPoint = this.sensor.MapSkeletonPointToDepth(
skelpoint,
kinect.DepthImageFormat.Resolution640x480Fps30);
return new Point(depthPoint.X, depthPoint.Y);
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, kinect.SkeletonFrameReadyEventArgs e)
{
if (isShooting || testState != TestState.None || calibrateState != CalibrateState.None)
{
return;
}
kinect.Skeleton[] skeletons = new kinect.Skeleton[0];
using (kinect.SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new kinect.Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
processSkeletons(skeletons);
}
void SensorDepthFrameReady(object sender, kinect.DepthImageFrameReadyEventArgs e)
{
if (isShooting)
{
return;
}
//if ((DateTime.Now - lastReadServos).TotalMilliseconds > 50.0d)
//{
// pololuMaestroConnector.TryGetTarget(ServoChannelMap.iAmHitInput);
// lastReadServos = DateTime.Now;
// this.Dispatcher.Invoke(new UpdateLabelDelegate(updatePmValuesLabel), string.Format("I Am Hit: {0:0.000}", posIAmHit));
// if (posIAmHit > 500 && lastPosIAmHit < 500)
// {
// shootBoth();
// }
// lastPosIAmHit = posIAmHit;
//}
using (kinect.DepthImageFrame depthFrame = e.OpenDepthImageFrame())
{
if (depthFrame != null)
{
// Copy the pixel data from the image to a temporary array
depthFrame.CopyPixelDataTo(this.depthPixels);
// Convert the depth to RGB
int colorPixelIndex = 0;
for (int i = 0; i < this.depthPixels.Length; ++i)
{
// discard the portion of the depth that contains only the player index
short depth = (short)(this.depthPixels[i] >> kinect.DepthImageFrame.PlayerIndexBitmaskWidth);
// to convert to a byte we're looking at only the lower 8 bits
// by discarding the most significant rather than least significant data
// we're preserving detail, although the intensity will "wrap"
// add 1 so that too far/unknown is mapped to black
byte intensity = (byte)((depth + 1) & byte.MaxValue);
// Write out blue byte
this.colorDepthPixels[colorPixelIndex++] = intensity;
// Write out green byte
this.colorDepthPixels[colorPixelIndex++] = intensity;
// Write out red byte
this.colorDepthPixels[colorPixelIndex++] = intensity;
// We're outputting BGR, the last byte in the 32 bits is unused so skip it
// If we were outputting BGRA, we would write alpha here.
++colorPixelIndex;
}
// Write the pixel data into our bitmap
this.colorBitmapDepth.WritePixels(
new Int32Rect(0, 0, this.colorBitmapDepth.PixelWidth, this.colorBitmapDepth.PixelHeight),
this.colorDepthPixels,
this.colorBitmapDepth.PixelWidth * sizeof(int),
0);
}
}
/*
//PlanarImage Image = e.ImageFrame.Image;
threatDetected = false;
int bubbleOuterRadiusMm = (int)bubbleScrollBar.Value;
int bubbleThicknessMm = (int)bubbleThicknessScrollBar.Value;
byte[] convertedDepthFrame = convertDepthFrame(Image.Bits, Image.Width, Image.Height, bubbleOuterRadiusMm, bubbleThicknessMm, out threatDetected);
depthImage.Source = BitmapSource.Create(
Image.Width, Image.Height, 96, 96, PixelFormats.Bgr32, null, convertedDepthFrame, Image.Width * 4);
*/
++totalFrames;
DateTime cur = DateTime.Now;
if (cur.Subtract(lastTime) > TimeSpan.FromSeconds(1))
{
int frameDiff = totalFrames - lastFrames;
lastFrames = totalFrames;
lastTime = cur;
frameRate.Text = frameDiff.ToString() + " fps";
}
}
