Now you can download a copy of these docs so you can use them offline! Download now
Kinect.cpp
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) FIRST 2011. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
5 /*----------------------------------------------------------------------------*/
6 
7 #include "Kinect.h"
8 
9 #include "DriverStation.h"
10 #include "NetworkCommunication/FRCComm.h"
11 #include "NetworkCommunication/UsageReporting.h"
12 #include "Skeleton.h"
13 #include "Synchronized.h"
14 #include "WPIErrors.h"
15 #include <cstring>
16 
17 #define kHeaderBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Header
18 #define kSkeletonExtraBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Extra1
19 #define kSkeletonBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Vertices1
20 
21 Kinect *Kinect::_instance = NULL;
22 
23 Kinect::Kinect() :
24  m_recentPacketNumber(0),
25  m_numberOfPlayers(0)
26 {
27  AddToSingletonList();
28  m_dataLock = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
29 
30  nUsageReporting::report(nUsageReporting::kResourceType_Kinect, 0);
31 }
32 
33 Kinect::~Kinect()
34 {
35  semTake(m_dataLock, WAIT_FOREVER);
36  semDelete(m_dataLock);
37 }
38 
44 {
45  if (_instance == NULL)
46  _instance = new Kinect();
47  return _instance;
48 }
49 
55 {
56  UpdateData();
57  return m_numberOfPlayers;
58 }
59 
65 {
66  UpdateData();
67  return m_floorClipPlane;
68 }
69 
75 {
76  UpdateData();
77  return m_gravityNormal;
78 }
79 
87 Skeleton Kinect::GetSkeleton(int skeletonIndex)
88 {
89  if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
90  {
91  wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
92  return Skeleton();
93  }
94  UpdateData();
95  return m_skeletons[skeletonIndex-1];
96 }
97 
104 {
105  if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
106  {
107  wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
108  return Point4();
109  }
110  UpdateData();
111  return m_position[skeletonIndex-1];
112 }
113 
120 uint32_t Kinect::GetQuality(int skeletonIndex)
121 {
122  if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
123  {
124  wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
125  return kClippedRight | kClippedLeft | kClippedTop | kClippedBottom;
126  }
127  UpdateData();
128  return m_quality[skeletonIndex-1];
129 }
130 
137 Kinect::SkeletonTrackingState Kinect::GetTrackingState(int skeletonIndex)
138 {
139  if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
140  {
141  wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
142  return kNotTracked;
143  }
144  UpdateData();
145  return m_trackingState[skeletonIndex-1];
146 }
147 
152 void Kinect::UpdateData()
153 {
154  Synchronized sync(m_dataLock);
155  uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
156  if (m_recentPacketNumber != packetNumber)
157  {
158  m_recentPacketNumber = packetNumber;
159  int retVal = getDynamicControlData(kHeaderBundleID, m_rawHeader, sizeof(m_rawHeader), 5);
160  if(retVal == 0)
161  {
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);
170  }
171 
172  retVal = getDynamicControlData(kSkeletonExtraBundleID, m_rawSkeletonExtra, sizeof(m_rawSkeletonExtra), 5);
173  if(retVal == 0)
174  {
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);
180  }
181 
182  retVal = getDynamicControlData(kSkeletonBundleID, m_rawSkeleton, sizeof(m_rawSkeleton), 5);
183  if(retVal == 0)
184  {
185  for(int i=0; i < Skeleton::JointCount; i++)
186  {
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];
191  }
192  }
193 
194  // TODO: Read skeleton #2
195  }
196 }
int GetNumberOfPlayers()
Definition: Kinect.cpp:54
Skeleton GetSkeleton(int skeletonIndex=1)
Definition: Kinect.cpp:87
uint32_t GetPacketNumber()
Point4 GetGravityNormal()
Definition: Kinect.cpp:74
Point4 GetPosition(int skeletonIndex=1)
Definition: Kinect.cpp:103
static Kinect * GetInstance()
Definition: Kinect.cpp:43
SkeletonTrackingState GetTrackingState(int skeletonIndex=1)
Definition: Kinect.cpp:137
static DriverStation * GetInstance()
uint32_t GetQuality(int skeletonIndex=1)
Definition: Kinect.cpp:120
Point4 GetFloorClipPlane()
Definition: Kinect.cpp:64
Definition: Kinect.h:24

Generated on Sat Apr 26 2014 12:26:45 for WPILibC++ by doxygen 1.8.6