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