9 #include "DriverStation.h"
10 #include "NetworkCommunication/FRCComm.h"
11 #include "NetworkCommunication/UsageReporting.h"
13 #include "Synchronized.h"
14 #include "WPIErrors.h"
17 #define kHeaderBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Header
18 #define kSkeletonExtraBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Extra1
19 #define kSkeletonBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Vertices1
21 Kinect *Kinect::_instance = NULL;
24 m_recentPacketNumber(0),
28 m_dataLock = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
30 nUsageReporting::report(nUsageReporting::kResourceType_Kinect, 0);
35 semTake(m_dataLock, WAIT_FOREVER);
36 semDelete(m_dataLock);
45 if (_instance == NULL)
57 return m_numberOfPlayers;
67 return m_floorClipPlane;
77 return m_gravityNormal;
89 if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
91 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"Skeleton index must be 1");
95 return m_skeletons[skeletonIndex-1];
105 if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
107 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"Skeleton index must be 1");
111 return m_position[skeletonIndex-1];
122 if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
124 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"Skeleton index must be 1");
125 return kClippedRight | kClippedLeft | kClippedTop | kClippedBottom;
128 return m_quality[skeletonIndex-1];
139 if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
141 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"Skeleton index must be 1");
145 return m_trackingState[skeletonIndex-1];
152 void Kinect::UpdateData()
156 if (m_recentPacketNumber != packetNumber)
158 m_recentPacketNumber = packetNumber;
159 int retVal = getDynamicControlData(kHeaderBundleID, m_rawHeader,
sizeof(m_rawHeader), 5);
162 m_numberOfPlayers = (int)m_rawHeader[13];
163 memcpy(&m_floorClipPlane.x, &m_rawHeader[18], 4);
164 memcpy(&m_floorClipPlane.y, &m_rawHeader[22], 4);
165 memcpy(&m_floorClipPlane.z, &m_rawHeader[26], 4);
166 memcpy(&m_floorClipPlane.w, &m_rawHeader[30], 4);
167 memcpy(&m_gravityNormal.x, &m_rawHeader[34], 4);
168 memcpy(&m_gravityNormal.y, &m_rawHeader[38], 4);
169 memcpy(&m_gravityNormal.z, &m_rawHeader[42], 4);
172 retVal = getDynamicControlData(kSkeletonExtraBundleID, m_rawSkeletonExtra,
sizeof(m_rawSkeletonExtra), 5);
175 memcpy(&m_position[0].x, &m_rawSkeletonExtra[22], 4);
176 memcpy(&m_position[0].y, &m_rawSkeletonExtra[26], 4);
177 memcpy(&m_position[0].z, &m_rawSkeletonExtra[30], 4);
178 memcpy(&m_quality[0], &m_rawSkeletonExtra[34], 4);
179 memcpy(&m_trackingState[0], &m_rawSkeletonExtra[38], 4);
182 retVal = getDynamicControlData(kSkeletonBundleID, m_rawSkeleton,
sizeof(m_rawSkeleton), 5);
185 for(
int i=0; i < Skeleton::JointCount; i++)
187 memcpy(&m_skeletons[0].m_joints[i].x, &m_rawSkeleton[i*12+2], 4);
188 memcpy(&m_skeletons[0].m_joints[i].y, &m_rawSkeleton[i*12+6], 4);
189 memcpy(&m_skeletons[0].m_joints[i].z, &m_rawSkeleton[i*12+10], 4);
190 m_skeletons[0].m_joints[i].trackingState = (Skeleton::JointTrackingState)m_rawSkeletonExtra[i+2];
Skeleton GetSkeleton(int skeletonIndex=1)
uint32_t GetPacketNumber()
Point4 GetGravityNormal()
Point4 GetPosition(int skeletonIndex=1)
static Kinect * GetInstance()
SkeletonTrackingState GetTrackingState(int skeletonIndex=1)
static DriverStation * GetInstance()
uint32_t GetQuality(int skeletonIndex=1)
Point4 GetFloorClipPlane()