001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007
008 package edu.wpi.first.wpilibj;
009
010 import com.sun.cldc.jna.Pointer;
011 import com.sun.cldc.jna.Structure;
012 import edu.wpi.first.wpilibj.communication.FRCControl;
013 import edu.wpi.first.wpilibj.communication.UsageReporting;
014
015 /**
016 * @author bradmiller
017 * Handles raw data input from the FRC Kinect Server
018 * when used with a Kinect device connected to the Driver Station.
019 * Each time a value is requested the most recent value is returned.
020 * See Getting Started with Microsoft Kinect for FRC and the Kinect
021 * for Windows SDK API reference for more information
022 *
023 */
024 public class Kinect {
025
026 private static Kinect m_instance;
027
028 /**
029 * Gets an instance of the Kinect device
030 *
031 * @return The Kinect.
032 */
033 public static synchronized Kinect getInstance() {
034 if(m_instance == null)
035 m_instance = new Kinect();
036 return m_instance;
037 }
038
039 /**
040 * A set of 4 coordinates (x,y,z,w) bundled into one object
041 */
042 public class Point4 {
043 public float x, y, z, w;
044
045 public float getX() { return x; }
046 public float getY() { return y; }
047 public float getZ() { return z; }
048 public float getW() { return w; }
049
050 public int size(){
051 return 16;
052 }
053 }
054
055 static class header_t extends Structure {
056
057 byte[] version = new byte[11];
058 byte players;
059 int flags;
060 float[] floorClipPlane = new float[4];
061 float[] gravityNormalVector = new float[3];
062
063 final static int size = 44;
064
065 header_t(Pointer backingMemory) {
066 useMemory(backingMemory);
067 }
068
069 public void read() {
070 backingNativeMemory.getBytes(0, version, 0, version.length);
071 players = backingNativeMemory.getByte(11);
072 flags = backingNativeMemory.getInt(12);
073 backingNativeMemory.getFloats(16, floorClipPlane, 0, floorClipPlane.length);
074 backingNativeMemory.getFloats(32, gravityNormalVector, 0, gravityNormalVector.length);
075 }
076
077 public void write() {
078 backingNativeMemory.setBytes(0, version, 0, version.length);
079 backingNativeMemory.setByte(11, players);
080 backingNativeMemory.setInt(12, flags);
081 backingNativeMemory.setFloats(16, floorClipPlane, 0, floorClipPlane.length);
082 backingNativeMemory.setFloats(32, gravityNormalVector, 0, gravityNormalVector.length);
083 }
084
085 public int size() {
086 return size;
087 }
088 }
089
090 static class skeletonExtra_t extends Structure {
091 byte[] trackingState = new byte[20];
092 float[] position = new float[3];
093 int quality;
094 int trackState;
095
096 final static int size = 40;
097
098 skeletonExtra_t(Pointer backingMemory) {
099 useMemory(backingMemory);
100 }
101
102 public void read() {
103 backingNativeMemory.getBytes(0, trackingState, 0, trackingState.length);
104 backingNativeMemory.getFloats(20, position, 0, position.length);
105 quality = backingNativeMemory.getInt(32);
106 trackState = backingNativeMemory.getInt(36);
107 }
108
109 public void write() {
110 backingNativeMemory.setBytes(0, trackingState, 0, trackingState.length);
111 backingNativeMemory.setFloats(20, position, 0, position.length);
112 backingNativeMemory.setInt(32, quality);
113 backingNativeMemory.setInt(36, trackState);
114 }
115
116 public int size() {
117 return size;
118 }
119 }
120
121 static class skeleton_t extends Structure {
122 float[] vertices = new float[60];
123
124 final static int size = 240;
125
126 skeleton_t(Pointer backingMemory) {
127 useMemory(backingMemory);
128 }
129
130 public void read() {
131 backingNativeMemory.getFloats(0, vertices, 0, vertices.length);
132 }
133
134 public void write() {
135 backingNativeMemory.setFloats(0, vertices, 0, vertices.length);
136 }
137
138 public int size() {
139 return size;
140 }
141 }
142
143 class header_block_t extends FRCControl.DynamicControlData {
144 byte size = 45;
145 byte id = kHeaderBlockID;
146 header_t data;
147
148 {
149 allocateMemory();
150 data = new header_t(
151 new Pointer(backingNativeMemory.address().toUWord().toPrimitive() + 2,
152 header_t.size));
153 }
154
155 public void read() {
156
157 size = backingNativeMemory.getByte(0);
158 id = backingNativeMemory.getByte(1);
159 data.read();
160 }
161
162 public void write() {
163 backingNativeMemory.setByte(0, size);
164 backingNativeMemory.setByte(1, id);
165 data.write();
166 }
167
168 public int size() {
169 return 46;
170 }
171
172 public void copy(header_block_t dest) {
173 write();
174 Pointer.copyBytes(backingNativeMemory, 0, dest.backingNativeMemory, 0, size());
175 dest.read();
176 }
177 }
178
179 class skeletonExtra_block_t extends FRCControl.DynamicControlData {
180 byte size = 41;
181 byte id = kSkeletonExtraBlockID;
182 skeletonExtra_t data;
183
184 {
185 allocateMemory();
186 data = new skeletonExtra_t(
187 new Pointer(backingNativeMemory.address().toUWord().toPrimitive() + 2,
188 skeletonExtra_t.size));
189 }
190
191 public void read() {
192
193 size = backingNativeMemory.getByte(0);
194 id = backingNativeMemory.getByte(1);
195 data.read();
196 }
197
198 public void write() {
199 backingNativeMemory.setByte(0, size);
200 backingNativeMemory.setByte(1, id);
201 data.write();
202 }
203
204 public int size() {
205 return 42;
206 }
207
208 public void copy(skeletonExtra_block_t dest) {
209 write();
210 Pointer.copyBytes(backingNativeMemory, 0, dest.backingNativeMemory, 0, size());
211 dest.read();
212 }
213 }
214
215 class skeleton_block_t extends FRCControl.DynamicControlData {
216 byte size = -15; //temporary hack for 241
217 byte id = kSkeletonBlockID;
218 skeleton_t data;
219
220 {
221 allocateMemory();
222 data = new skeleton_t(
223 new Pointer(backingNativeMemory.address().toUWord().toPrimitive() + 2,
224 skeleton_t.size));
225 }
226
227 public void read() {
228 size = backingNativeMemory.getByte(0);
229 id = backingNativeMemory.getByte(1);
230 data.read();
231 }
232
233 public void write() {
234 backingNativeMemory.setByte(0, size);
235 backingNativeMemory.setByte(1, id);
236 data.write();
237 }
238
239 public int size() {
240 return 242;
241 }
242
243 public void copy(skeleton_block_t dest) {
244 write();
245 Pointer.copyBytes(backingNativeMemory, 0, dest.backingNativeMemory, 0, size());
246 dest.read();
247 }
248 }
249
250 static final byte kHeaderBlockID = 19;
251 static final byte kSkeletonExtraBlockID = 20;
252 static final byte kSkeletonBlockID = 21;
253
254 header_block_t m_headerData;
255 skeletonExtra_block_t m_skeletonExtraData;
256 skeleton_block_t m_skeletonData;
257 boolean m_headerValid = false;
258 boolean m_skeletonExtraValid = false;
259 boolean m_skeletonValid = false;
260 final Object m_headerDataSemaphore;
261 final Object m_skeletonExtraDataSemaphore;
262 final Object m_skeletonDataSemaphore;
263 int m_recentPacketNumber = 0;
264
265 /**
266 * Kinect constructor.
267 *
268 * This is only called once on the first call of getInstance()
269 */
270 Kinect() {
271 m_headerData = new header_block_t();
272 m_skeletonExtraData = new skeletonExtra_block_t();
273 m_skeletonData = new skeleton_block_t();
274 m_headerDataSemaphore = new Object();
275 m_skeletonExtraDataSemaphore = new Object();
276 m_skeletonDataSemaphore = new Object();
277
278 UsageReporting.report(UsageReporting.kResourceType_Kinect, 0);
279 }
280 header_block_t tempHeaderData = new header_block_t();
281 skeletonExtra_block_t tempSkeletonExtraData = new skeletonExtra_block_t();
282 skeleton_block_t tempSkeletonData = new skeleton_block_t();
283
284
285 /**
286 * Called by the other Kinect functions to check for the latest data
287 * This function will update the internal data structures
288 * with the most recent Kinect input
289 */
290 void updateData() {
291 int retVal;
292
293 if (m_recentPacketNumber != DriverStation.getInstance().getPacketNumber()){
294 m_recentPacketNumber = DriverStation.getInstance().getPacketNumber();
295 synchronized (m_headerDataSemaphore) {
296 retVal = FRCControl.getDynamicControlData(kHeaderBlockID, tempHeaderData, tempHeaderData.size(), 5);
297 if (retVal == 0) {
298 tempHeaderData.copy(m_headerData);
299 m_headerValid = true;
300 }else {
301 m_headerValid = false;
302 }
303 }
304
305 synchronized (m_skeletonExtraDataSemaphore) {
306 retVal = FRCControl.getDynamicControlData(kSkeletonExtraBlockID, tempSkeletonExtraData, tempSkeletonExtraData.size(), 5);
307 if (retVal == 0) {
308 tempSkeletonExtraData.copy(m_skeletonExtraData);
309 m_skeletonExtraValid = true;
310 }else {
311 m_skeletonExtraValid = false;
312 }
313 }
314
315 synchronized (m_skeletonDataSemaphore) {
316 retVal = FRCControl.getDynamicControlData(kSkeletonBlockID, tempSkeletonData, tempSkeletonData.size(), 5);
317 if (retVal == 0) {
318 tempSkeletonData.copy(m_skeletonData);
319 m_skeletonValid = true;
320 }else {
321 m_skeletonValid = false;
322 }
323 }
324 }
325 }
326
327 /**
328 * Query the number of players detected by the Kinect
329 *
330 * @return The current number of players
331 */
332 public int getNumberOfPlayers() {
333 updateData();
334 if (!m_headerValid){
335 return 0;
336 }
337 synchronized (m_headerDataSemaphore){
338 return (int)m_headerData.data.players;
339 }
340 }
341
342 /**
343 * Retrieve the FloorClipPlane from the Kinect device
344 *
345 * @return The FloorClipPlane
346 */
347 public Point4 getFloorClipPlane() {
348 updateData();
349 Point4 tempClipPlane = new Point4();
350
351 if (!m_headerValid){
352 return tempClipPlane;
353 }
354
355 synchronized(m_headerDataSemaphore){
356 tempClipPlane.x = m_headerData.data.floorClipPlane[0];
357 tempClipPlane.y = m_headerData.data.floorClipPlane[1];
358 tempClipPlane.z = m_headerData.data.floorClipPlane[2];
359 tempClipPlane.w = m_headerData.data.floorClipPlane[3];
360 }
361 return tempClipPlane;
362 }
363
364 /**
365 * Retrieve the GravityNormal vector from the Kinect device
366 * The w value returned from this method is always 0
367 * @return The GravityNormal vector
368 */
369 public Point4 getGravityNormal() {
370 updateData();
371 Point4 tempGravityNormal = new Point4();
372
373 if (!m_headerValid){
374 return tempGravityNormal;
375 }
376
377 synchronized(m_headerDataSemaphore){
378 tempGravityNormal.x = m_headerData.data.gravityNormalVector[0];
379 tempGravityNormal.y = m_headerData.data.gravityNormalVector[1];
380 tempGravityNormal.z = m_headerData.data.gravityNormalVector[2];
381 tempGravityNormal.w = 0;
382 }
383 return tempGravityNormal;
384 }
385
386 /**
387 * Query the position of the detected skeleton
388 * The w value returned from this method is always 1
389 * @return The position of the skeleton
390 */
391 public Point4 getPosition() {
392 updateData();
393 Point4 tempPosition = new Point4();
394
395 if (!m_skeletonExtraValid){
396 return tempPosition;
397 }
398
399 synchronized(m_headerDataSemaphore){
400 tempPosition.x = m_skeletonExtraData.data.position[0];
401 tempPosition.y = m_skeletonExtraData.data.position[1];
402 tempPosition.z = m_skeletonExtraData.data.position[2];
403 tempPosition.w = 1;
404 }
405 return tempPosition;
406 }
407
408 /**
409 * Retrieve the detected skeleton from the Kinect device
410 *
411 * @return The skeleton
412 */
413 public Skeleton getSkeleton() {
414 updateData();
415 Skeleton tempSkeleton = new Skeleton();
416
417 if (!m_skeletonValid){
418 return tempSkeleton;
419 }
420
421 synchronized (m_skeletonDataSemaphore){
422 for(int i=0; i<20; i++){
423 tempSkeleton.skeleton[i].x = m_skeletonData.data.vertices[i*3];
424 tempSkeleton.skeleton[i].y = m_skeletonData.data.vertices[i*3+1];
425 tempSkeleton.skeleton[i].z = m_skeletonData.data.vertices[i*3+2];
426 }
427 }
428
429 synchronized (m_skeletonExtraDataSemaphore){
430 for(int i=0; i<20; i++){
431 tempSkeleton.skeleton[i].trackingState = m_skeletonExtraData.data.trackingState[i];
432 }
433 switch(m_skeletonExtraData.data.trackState){
434 case 0:
435 tempSkeleton.trackState = Skeleton.tTrackState.kNotTracked;
436 break;
437 case 1:
438 tempSkeleton.trackState = Skeleton.tTrackState.kPositionOnly;
439 break;
440 case 2:
441 tempSkeleton.trackState = Skeleton.tTrackState.kTracked;
442 break;
443 }
444
445 }
446 return tempSkeleton;
447 }
448 }