/// <summary> /// Called when the heart beat timer expires. Sends a "blank" packet with player /// count and Joystick data set to 0. /// </summary> /// <param name="stateInfo"></param> private void HeartBeatExpired(Object stateInfo) { sbyte[] nullAxis = new sbyte[6]; m_version1Packet.PlayerCount.Set(0); m_version1Packet.Joystick1.Set(nullAxis, 0); m_version1Packet.Joystick2.Set(nullAxis, 0); Send(); NTKinect.UpdateJoysticks(0.0, 0.0, 0.0, 0.0); }
/// <summary> /// Manages sending Kinect information as UDP to the specified hostname and port /// at least once a second. /// /// Uses the FIRST gesture processor to process gestures into joysticks. /// </summary> /// <param name="kinectVersion">The version string to report to the Driver Station.</param> /// <param name="hostname">The destination hostname.</param> /// <param name="port">The destination port number.</param> public KinectProtocol_v1Manager(String kinectVersion, String hostname, int port) { m_kinectVersion = kinectVersion; m_kinectStatus = "No Kinect"; m_hostname = hostname; m_port = port; m_udpClient = new UdpClient(); m_version1Packet = new KinectProtocol_v1(); m_heartbeatTimer = new Timer(this.HeartBeatExpired); m_heartbeatTimer.Change(HEARTBEAT_PERIOD_MS, HEARTBEAT_PERIOD_MS); m_gestureProcessor = new FIRSTGestureProcessor(); NTKinect.Init(); }
/// <summary> /// Cuts a new UDP packet and sends it to the Driver Station. /// </summary> private void Send() { using (MemoryStream udpbuffer = new MemoryStream()) { NetworkOrderBinaryWriter writer = new NetworkOrderBinaryWriter(udpbuffer); lock (m_version1Packet) { m_version1Packet.VersionNumber.Set(m_kinectStatus); m_version1Packet.Serialize(writer); NTKinect.UpdateFromPacket(m_version1Packet); } m_udpClient.Send(udpbuffer.GetBuffer(), (int)udpbuffer.Length, m_hostname, m_port); } }
/// <summary> /// Processes the given skeleton and updates the network packet data structure, then /// sends the resulting packet. /// </summary> /// <param name="frame">The Kinect Skeleton frame to process.</param> public void ProcessSkeletonData(SkeletonFrame frame) { if ((m_skeletonArray == null) || (m_skeletonArray.Length != frame.SkeletonArrayLength)) { m_skeletonArray = new Skeleton[frame.SkeletonArrayLength]; } frame.CopySkeletonDataTo(m_skeletonArray); // Do stuff here lock (m_version1Packet) { UpdatePacketGlobalData(frame); // Get the best skeleton Skeleton skeleton = KinectUtils.SelectBestSkeleton(m_skeletonArray); UpdatePacketSkeletonData(skeleton); UpdatePacketJoystickData(skeleton); } Send(); m_heartbeatTimer.Change(HEARTBEAT_PERIOD_MS, HEARTBEAT_PERIOD_MS); NTKinect.UpdateHeartBeat(); }
/// <summary> /// Processes a skeleton into joystick data using the default FIRST gestures. /// </summary> /// <param name="joy">Vector of Joysticks to put the result in</param> /// <param name="skeleton">The skeleton to process</param> public void ProcessGestures(Networking.WritableElements.WritableJoystick[] joy, Microsoft.Kinect.Skeleton skeleton) { // Check edge cases if (joy == null || joy.Length < 2 || joy[0] == null || joy[1] == null) { return; } sbyte[] leftAxis = new sbyte[6]; sbyte[] rightAxis = new sbyte[6]; sbyte[] nullAxis = new sbyte[6]; bool dataWithinExpectedRange; ushort buttons = 0; double leftAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderLeft].Position, skeleton.Joints[JointType.WristLeft].Position, true)); double rightAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderRight].Position, skeleton.Joints[JointType.WristRight].Position)); dataWithinExpectedRange = leftAngle <ARM_MAX_ANGLE && leftAngle> ARM_MIN_ANGLE && rightAngle <ARM_MAX_ANGLE && rightAngle> ARM_MIN_ANGLE; double leftYAxis = CoerceToRange(leftAngle, -70, 70, -127, 128); double rightYAxis = CoerceToRange(rightAngle, -70, 70, -127, 128); dataWithinExpectedRange = dataWithinExpectedRange && InSameZPlane(skeleton.Joints[JointType.ShoulderLeft].Position, skeleton.Joints[JointType.WristLeft].Position, Z_PLANE_TOLERANCE) && InSameZPlane(skeleton.Joints[JointType.ShoulderRight].Position, skeleton.Joints[JointType.WristRight].Position, Z_PLANE_TOLERANCE); // Head buttons double headAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderCenter].Position, skeleton.Joints[JointType.Head].Position)); if (IsHeadRight(headAngle)) { buttons |= (ushort)WritableJoystick.Buttons.Btn1; } if (IsHeadLeft(headAngle)) { buttons |= (ushort)WritableJoystick.Buttons.Btn2; } // Right Leg XY Button double rightLegAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.HipRight].Position, skeleton.Joints[JointType.AnkleRight].Position)); if (IsLegOut(rightLegAngle)) { buttons |= (ushort)WritableJoystick.Buttons.Btn3; } // Left Leg XY Button double leftLegAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.HipLeft].Position, skeleton.Joints[JointType.AnkleLeft].Position, true)); if (IsLegOut(leftLegAngle)) { buttons |= (ushort)WritableJoystick.Buttons.Btn4; } // Right Leg YZ Buttons double rightLegYZ = RadToDeg(AngleYZ(skeleton.Joints[JointType.HipRight].Position, skeleton.Joints[JointType.AnkleRight].Position)); if (IsLegForward(rightLegYZ)) { buttons |= (ushort)WritableJoystick.Buttons.Btn5; } if (IsLegBackward(rightLegYZ)) { buttons |= (ushort)WritableJoystick.Buttons.Btn6; } // Left Leg YZ Buttons double leftLegYZ = RadToDeg(AngleYZ(skeleton.Joints[JointType.HipLeft].Position, skeleton.Joints[JointType.AnkleLeft].Position)); if (IsLegForward(leftLegYZ)) { buttons |= (ushort)WritableJoystick.Buttons.Btn7; } if (IsLegBackward(leftLegYZ)) { buttons |= (ushort)WritableJoystick.Buttons.Btn8; } if (dataWithinExpectedRange) { // Invert joystick axis to match a real joystick // (pushing away creates negative values) leftAxis[(uint)WritableJoystick.Axis.Y] = (sbyte)-leftYAxis; rightAxis[(uint)WritableJoystick.Axis.Y] = (sbyte)-rightYAxis; //Use "Button 9" as Kinect control enabled signal buttons |= (ushort)WritableJoystick.Buttons.Btn9; joy[0].Set(leftAxis, buttons); joy[1].Set(rightAxis, buttons); NTKinect.UpdateJoysticks(0.0, leftYAxis, 0.0, rightYAxis); } else { joy[0].Set(nullAxis, 0); joy[1].Set(nullAxis, 0); NTKinect.UpdateJoysticks(0.0, 0.0, 0.0, 0.0); } }