int main(int argc, char **argv) { ros::init(argc, argv, "openni_tracker"); ros::NodeHandle nh; string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml"; XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); CHECK_RC(nRetVal, "InitFromXml"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } XnCallbackHandle hUserCallbacks; g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); XnCallbackHandle hCalibrationCallbacks; g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } XnCallbackHandle hPoseCallbacks; g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); ros::Rate r(30); ros::NodeHandle pnh("~"); string frame_id("openni_depth_frame"); pnh.getParam("camera_frame_id", frame_id); image_transport::ImageTransport it(nh); image_transport::Publisher pub = it.advertise("people_segmentation_image", 1); while (ros::ok()) { g_Context.WaitAndUpdateAll(); publishTransforms(frame_id, pub); r.sleep(); } g_Context.Shutdown(); return 0; }
// this function will print the confidence values of all the joints void jointConf() { XnUserID nId[15]; XnUInt16 oId = 15; g_UserGenerator.GetUsers(nId, oId); int userSum = 0; float userConfidence = 0; // ITERATE THROUGH ALL TRACKED USERS AND OUTPUT INDIVIDUAL JOINT CONFIDENCE VALUES for (int i = 0; i < oId; ++i){ if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(nId[i])){ XnSkeletonJointPosition jointData; printf("\n"); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_HEAD, jointData); std::cout << "User " << nId[i] << " - Head " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_NECK, jointData); std::cout << "User " << nId[i] << " - Neck " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_TORSO, jointData); std::cout << "User " << nId[i] << " - Torso " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_SHOULDER, jointData); std::cout << "User " << nId[i] << " - L Shoulder " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_ELBOW, jointData); std::cout << "User " << nId[i] << " - L Elbow " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HAND, jointData); std::cout << "User " << nId[i] << " - L Hand " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_SHOULDER, jointData); std::cout << "User " << nId[i] << " - R Shoulder " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_ELBOW, jointData); std::cout << "User " << nId[i] << " - R Elbow " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HAND, jointData); std::cout << "User " << nId[i] << " - R Hand " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HIP, jointData); std::cout << "User " << nId[i] << " - L Hip " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_KNEE, jointData); std::cout << "User " << nId[i] << " - L Knee " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_FOOT, jointData); std::cout << "User " << nId[i] << " - L Foot " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HIP, jointData); std::cout << "User " << nId[i] << " - R Hip " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_KNEE, jointData); std::cout << "User " << nId[i] << " - R Knee " << jointData.fConfidence << std::endl; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_FOOT, jointData); std::cout << "User " << nId[i] << " - R Foot " << jointData.fConfidence << std::endl; } } }
void glut_display() { xn::DepthMetaData pDepthMapMD; xn::ImageMetaData pImageMapMD; XnUserID pUser[2]; XnUInt16 nUsers=2; #ifdef DEBUGOUT ofstream datei; #endif glEnable(GL_TEXTURE_2D); pUser[0] = 0; pUser[1] = 0; glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glLoadIdentity(); /*glFrustum( -ROOM_X/2+(1500.0/1750.0)*static_cast<int>(headtrans.x), ROOM_X/2+(1500.0/1750.0)*static_cast<int>(headtrans.x), -ROOM_Y/2-(1500.0/1750.0)*static_cast<int>(headtrans.y), ROOM_Y/2-(1500.0/1750.0)*static_cast<int>(headtrans.y), 1525, 2525);*/ float nearplane = 1525; float screenaspect = ROOM_X/ROOM_Y; glFrustum( nearplane*(-0.5 * screenaspect + headtrans.x)/headtrans.z, nearplane*( 0.5 * screenaspect + headtrans.x)/headtrans.z, nearplane*(-0.5 + headtrans.y)/headtrans.z, nearplane*( 0.5 + headtrans.y)/headtrans.z, nearplane, 2525); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); nRetVal = context.WaitAndUpdateAll(); checkError("Fehler beim Aktualisieren der Daten", nRetVal); /* Anzahl der User auslesen und in Objekten speichern */ user.GetUsers(pUser, nUsers); if(pUser[0]!=0 && pUserOld!=1) { cout << "User 1 erkannt" << endl; pUserOld=1; } xn::SkeletonCapability pSkeleton = user.GetSkeletonCap(); XnCallbackHandle hnd; pSkeleton.RegisterCalibrationCallbacks(skel_cal_start, skel_cal_end, 0,hnd); if(calibration){ } pSkeleton.SetSkeletonProfile(XN_SKEL_PROFILE_ALL); if(pSkeleton.IsCalibrated(pUser[0])) { XnSkeletonJointTransformation head; pSkeleton.StartTracking(pUser[0]); pSkeleton.GetSkeletonJoint(pUser[0], XN_SKEL_HEAD, head); if(head.position.fConfidence && head.orientation.fConfidence) { /* headtrans.rotmat[0] = head.orientation.orientation.elements[0]; headtrans.rotmat[1] = head.orientation.orientation.elements[1]; headtrans.rotmat[2] = head.orientation.orientation.elements[2]; headtrans.rotmat[3] = 0; headtrans.rotmat[4] = head.orientation.orientation.elements[3]; headtrans.rotmat[5] = head.orientation.orientation.elements[4]; headtrans.rotmat[6] = head.orientation.orientation.elements[5]; headtrans.rotmat[7] = 0; headtrans.rotmat[8] = -head.orientation.orientation.elements[6]; headtrans.rotmat[9] = -head.orientation.orientation.elements[7]; headtrans.rotmat[10] =-head.orientation.orientation.elements[8]; headtrans.rotmat[11] = 0; headtrans.rotmat[12] = 0; headtrans.rotmat[13] = 0; headtrans.rotmat[14] = 0; headtrans.rotmat[15] = 1;*/ headtrans.x = head.position.position.X; headtrans.y = head.position.position.Y; headtrans.z = head.position.position.Z; #ifdef CONSOLEOUT clearScreen(); cout << "Confidence Position: " << head.position.fConfidence << " X: " << head.position.position.X << " Y: " << head.position.position.Y << " Z: " << head.position.position.Z << endl << "Confidence Rotation: " << head.orientation.fConfidence << endl << "\t" << headtrans.rotmat[0] << "\t" << headtrans.rotmat[4] << "\t" << headtrans.rotmat[8] << endl << "\t" << headtrans.rotmat[1] << "\t" << headtrans.rotmat[5] << "\t" << headtrans.rotmat[9] << endl << "\t" << headtrans.rotmat[2] << "\t" << headtrans.rotmat[6] << "\t" << headtrans.rotmat[10] << endl << endl; #endif } } //------------------------------------------------------------------------------------------ //BEGIN: Kamera-Test //------------------------------------------------------------------------------------------ //glTranslatef(0,0,-2000); glTranslatef(headtrans.x,headtrans.y,1-headtrans.z); cout << headtrans.x << " " << headtrans.y << " " << headtrans.z << endl; cout << nearplane*(-0.5 * screenaspect + headtrans.x)/headtrans.z << " " << nearplane*( 0.5 * screenaspect + headtrans.x)/headtrans.z << " " << nearplane*(-0.5 + headtrans.y)/headtrans.z << " " << nearplane*( 0.5 + headtrans.y)/headtrans.z << " " << endl; draw_room(); //glPushAttrib(GL_ALL_ATTRIB_BITS); //glutSolidCube(5); //glPopAttrib(); //------------------------------------------------------------------------------------------ glDisable(GL_TEXTURE_2D); glutSwapBuffers(); #ifdef DEBUGOUT if(printFile) { printFile=false; datei.close(); } #endif }
void writeSkeleton() { string s; XnUserID aUsers[15]; XnUInt16 nUsers = 15; s += "{\"skeletons\":["; int skeletons = 0; int activeUsers = 0; int totalJoints; string userSkeleton; string activeSkeletons[nUsers]; userGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { totalJoints = 0; userSkeleton = ""; char tmp[1024]; sprintf(tmp, "{\"userid\":%d,\"joints\":[", aUsers[i]); userSkeleton += tmp; if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) { writeJoint(&userSkeleton, "head", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) { writeJoint(&userSkeleton, "neck", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) { writeJoint(&userSkeleton, "l_collar", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) { writeJoint(&userSkeleton, "l_shoulder", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) { writeJoint(&userSkeleton, "l_elbow", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) { writeJoint(&userSkeleton, "l_wrist", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) { writeJoint(&userSkeleton, "l_hand", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) { writeJoint(&userSkeleton, "l_fingertop", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) { writeJoint(&userSkeleton, "r_collar", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) { writeJoint(&userSkeleton, "r_shoulder", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) { writeJoint(&userSkeleton, "r_elbow", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) { writeJoint(&userSkeleton, "r_wrist", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) { writeJoint(&userSkeleton, "r_hand", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) { writeJoint(&userSkeleton, "r_fingertip", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) { writeJoint(&userSkeleton, "torso", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) { writeJoint(&userSkeleton, "waist", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) { writeJoint(&userSkeleton, "l_hip", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) { writeJoint(&userSkeleton, "l_knee", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) { writeJoint(&userSkeleton, "l_ankle", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0) { writeJoint(&userSkeleton, "l_foot", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0) { writeJoint(&userSkeleton, "r_hip", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0) { writeJoint(&userSkeleton, "r_knee", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0) { writeJoint(&userSkeleton, "r_ankle", jointCoords); totalJoints++; } if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0) { writeJoint(&userSkeleton, "r_foot", jointCoords); totalJoints++; } if(totalJoints > 0){ userSkeleton.erase(userSkeleton.length()-1, 1); } } userSkeleton += "]}"; if(totalJoints > 0){ activeSkeletons[i] = userSkeleton; skeletons++; } } string concatSkeletons; concatSkeletons = ""; for(int ai = 0; ai < skeletons; ++ai) { if(ai != 0){ concatSkeletons += ","; } concatSkeletons += activeSkeletons[ai]; } s += concatSkeletons; // add a timestamp char tmp[1024]; sprintf(tmp, "],\"elapsed\":%.3f}", clockAsFloat(last)); s += tmp; if (skeletons > 0) { cout << s; cout << endl; cout.flush(); } else { s.clear(); } skeletons=0; }
int main(int argc, char **argv) { XnStatus rc = XN_STATUS_OK; rc = g_Context.InitFromXmlFile(SAMPLE_XML_PATH); CHECK_RC(rc, "InitFromXml"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(rc, "Find depth generator"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator); CHECK_RC(rc, "Find Image generator"); rc = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); CHECK_RC(rc, "Find user generator"); // Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator); //g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint(); if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON) || !g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("User generator doesn't support either skeleton or pose detection.\n"); return XN_STATUS_ERROR; } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); rc = g_Context.StartGeneratingAll(); CHECK_RC(rc, "StartGenerating"); XnCallbackHandle hUserCBs, hCalibrationCBs, hPoseCBs; g_UserGenerator.RegisterUserCallbacks(NewUser, LostUser, NULL, hUserCBs); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(CalibrationStarted, CalibrationEnded, NULL, hCalibrationCBs); g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(PoseDetected, NULL, NULL, hPoseCBs); #ifdef USE_GLUT // Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator); //g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint(); glInit(&argc, argv); glutMainLoop(); #else if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context)) { printf("Error initing opengles\n"); CleanupExit(); } glDisable(GL_DEPTH_TEST); // glEnable(GL_TEXTURE_2D); glEnableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_COLOR_ARRAY); while ((!_kbhit()) && (!g_bQuit)) { glutDisplay(); eglSwapBuffers(display, surface); } opengles_shutdown(display, surface, context); CleanupExit(); #endif }
// Callback: New user was detected void XN_CALLBACK_TYPE new_user(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { printf("{\"found_user\":{\"userid\":%d}, \"elapsed\":%.3f}\n", nId, clockAsFloat(last)); userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); }
void XN_CALLBACK_TYPE user_reenter(xn::UserGenerator& generator, XnUserID nId, void *pCookie) { printf("{\"user_reenter\": {\"userid\": %d}}\n", nId); userGenerator.GetSkeletonCap().StartTracking(nId); }
void matrixCalc(void *outputs) { TML::Matrix out1(outputs, 0); TML::Matrix out2(outputs, 1); TML::Matrix out3(outputs, 2); TML::Matrix out4(outputs, 3); xn::DepthMetaData depthMD; xn::SceneMetaData sceneMD; xn::ImageMetaData imageMD; depth.GetMetaData(depthMD); user.GetUserPixels(0, sceneMD); image.GetMetaData(imageMD); context.WaitNoneUpdateAll(); t_jit_matrix_info tmi; memset(&tmi, 0, sizeof(tmi)); tmi.dimcount = 2; tmi.planecount = 1; tmi.dimstride[0] = 4; tmi.dimstride[1] = depthMD.XRes()*4; int width = tmi.dim[0] = depthMD.XRes(); int height = tmi.dim[1] = depthMD.YRes(); tmi.type = _jit_sym_float32; out1.resizeTo(&tmi); tmi.planecount = 1; tmi.dimstride[0] = 1; tmi.dimstride[1] = depthMD.XRes(); tmi.type = _jit_sym_char; out2.resizeTo(&tmi); tmi.planecount = 4; tmi.dimstride[0] = 4; tmi.dimstride[1] = depthMD.XRes()*4; tmi.type = _jit_sym_char; out3.resizeTo(&tmi); const XnDepthPixel* pDepth = depthMD.Data(); float *depthData = (float*)out1.data(); //Copy depth data int x,y; for (y=0; y<height; y++) { for (x=0; x<width; x++) { depthData[0] = (float)pDepth[0]/powf(2, 15); depthData++; pDepth++; } } //Get the users unsigned char *userData = (unsigned char*)out2.data(); const XnLabel* pLabels = sceneMD.Data(); for (y=0; y<height; y++) { for (x=0; x<width; x++) { userData[0] = pLabels[0]; userData++; pLabels++; } } //Get the colors const XnRGB24Pixel* pPixels = imageMD.RGB24Data(); unsigned char *pixData = (unsigned char*)out3.data(); for (y=0; y<height; y++) { for (x=0; x<width; x++) { pixData[0] = 0; pixData[1] = pPixels[0].nRed; pixData[2] = pPixels[0].nGreen; pixData[3] = pPixels[0].nBlue; pixData+=4; pPixels++; } } //For all the users -- output the joint info... XnUserID aUsers[15]; XnUInt16 nUsers = 15; user.GetUsers(aUsers, nUsers); int rUsers = 0; xn::SkeletonCapability sc = user.GetSkeletonCap(); int i; for (i=0; i<nUsers; i++) { if (user.GetSkeletonCap().IsTracking(aUsers[i])) rUsers++; } tmi.dimcount = 2; tmi.planecount = 3; tmi.dimstride[0] = 3*4; tmi.dimstride[1] = 24*3*4; tmi.dim[0] = 24; tmi.dim[1] = rUsers; tmi.type = _jit_sym_float32; out4.resizeTo(&tmi); float *sData = (float*)out4.data(); if (rUsers == 0) { int n; for (n=0; n<24; n++) { sData[0] = 0; sData[1] = 0; sData[2] = 0; sData+=3; } } else { for (i=0; i<nUsers; i++) { if (user.GetSkeletonCap().IsTracking(aUsers[i])) { int n; for (n=0; n<24; n++) { XnSkeletonJointPosition jp; user.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], (XnSkeletonJoint)(n+1), jp); sData[0] = (1280 - jp.position.X) / 2560; sData[1] = (1280 - jp.position.Y) / 2560; sData[2] = jp.position.Z * 7.8125 / 10000; // if (n == 0) // { // post("%f %f %f\n", sData[0], sData[1], sData[2]); // } sData+=3; } } } } //post("%i\n", rUsers); }
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) { post("New User %d\n", nId); generator.GetSkeletonCap().RequestCalibration(nId, TRUE); }
void NiAnimation::updateSkeletonAnimation(xn::UserGenerator &user_generator) { XnUInt16 num_users = MAX_USERS; user_generator.GetUsers(all_users, num_users); // tracking only default (first) person if (!user_generator.GetSkeletonCap().IsTracking(DEFAULT_USER)) return; // POSITION // animik: x,y,z € <-196, 196> XnSkeletonJointPosition torso; user_generator.GetSkeletonCap().GetSkeletonJointPosition(all_users[0], XN_SKEL_TORSO, torso); if (torso.fConfidence == 1) setPosition(torso.position.X*MM0, torso.position.Y*MM0+40, 150-torso.position.Z*MM0); // ROTATION double x, y, z, tx, ty, tz; // hip -> abdomen -> chest -> ... if (calculateRotation(user_generator, XN_SKEL_LEFT_HIP, tx, ty, tz, MM1) && calculateRotation(user_generator, XN_SKEL_RIGHT_HIP, x, y, z, MM1)) setRotation(getNodeByName("hip"), (x+tx)/2, (y+ty)/2, (z+tz)/2); if (calculateRotation(user_generator, XN_SKEL_TORSO, x, y, z, MM1)) setRotation(getNodeByName("abdomen"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_NECK, tx, ty, tz, MM1)) // !!! + torso ^ setRotation(getNodeByName("chest"), (x+tx)/2, (y+ty)/2, (z+tz)/2); // ... -> neck -> head if (calculateRotation(user_generator, XN_SKEL_NECK, x, y, z, MM1)) setRotation(getNodeByName("neck"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_HEAD, x, y, z, MM1)) setRotation(getNodeByName("head"), x, y, z); if (!global_mirror) { // ... -> lCollar -> lShldr -> lForeArm -> lHand if (calculateRotation(user_generator, XN_SKEL_LEFT_COLLAR, x, y, z, MM2)) setRotation(getNodeByName("rCollar"), x, y, z/MM2); if (calculateRotation(user_generator, XN_SKEL_LEFT_SHOULDER, x, y, z, MM2)) setRotation(getNodeByName("rShldr"), x, y*MM2, z/MM2); if (calculateRotation(user_generator, XN_SKEL_LEFT_ELBOW, x, y, z, MM2)) setRotation(getNodeByName("rForeArm"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_LEFT_HAND, x, y, z, MM2)) setRotation(getNodeByName("rHand"), x, y, z/MM2); // ... -> rCollar -> rShldr -> rForeArm -> rHand if (calculateRotation(user_generator, XN_SKEL_RIGHT_COLLAR, x, y, z, MM2)) setRotation(getNodeByName("lCollar"), x, y, z/MM2); if (calculateRotation(user_generator, XN_SKEL_RIGHT_SHOULDER, x, y, z, MM2)) setRotation(getNodeByName("lShldr"), x, y*MM2, z/MM2); if (calculateRotation(user_generator, XN_SKEL_RIGHT_ELBOW, x, y, z, MM2)) setRotation(getNodeByName("lForeArm"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_RIGHT_HAND, x, y, z, MM2)) setRotation(getNodeByName("lHand"), x, y, z/MM2); // hip -> lThigh -> lShin -> lFoot if (calculateRotation(user_generator, XN_SKEL_RIGHT_HIP, x, y, z, MM3) ) setRotation(getNodeByName("lThigh"), x/MM3, y, z); if (calculateRotation(user_generator, XN_SKEL_RIGHT_KNEE, x, y, z, MM3)) setRotation(getNodeByName("lShin"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_RIGHT_FOOT, x, y, z, MM3)) setRotation(getNodeByName("lFoot"), x/MM3, y, z); // hip -> rThigh -> rShin -> rFoot if (calculateRotation(user_generator, XN_SKEL_LEFT_HIP, x, y, z, MM3)) setRotation(getNodeByName("rThigh"), x/MM3, y, z); if (calculateRotation(user_generator, XN_SKEL_LEFT_KNEE, x, y, z, MM3)) setRotation(getNodeByName("rShin"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_LEFT_FOOT, x, y, z, MM3)) setRotation(getNodeByName("rFoot"), x/MM3, y, z); } else { // ... -> lCollar -> lShldr -> lForeArm -> lHand if (calculateRotation(user_generator, XN_SKEL_LEFT_COLLAR, x, y, z, MM2)) setRotation(getNodeByName("lCollar"), x, y, z/MM2); if (calculateRotation(user_generator, XN_SKEL_LEFT_SHOULDER, x, y, z, MM2)) setRotation(getNodeByName("lShldr"), x, y*MM2, z/MM2); if (calculateRotation(user_generator, XN_SKEL_LEFT_ELBOW, x, y, z, MM2)) setRotation(getNodeByName("lForeArm"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_LEFT_HAND, x, y, z, MM2)) setRotation(getNodeByName("lHand"), x, y, z/MM2); // ... -> rCollar -> rShldr -> rForeArm -> rHand if (calculateRotation(user_generator, XN_SKEL_RIGHT_COLLAR, x, y, z, MM2)) setRotation(getNodeByName("rCollar"), x, y, z/MM2); if (calculateRotation(user_generator, XN_SKEL_RIGHT_SHOULDER, x, y, z, MM2)) setRotation(getNodeByName("rShldr"), x, y*MM2, z/MM2); if (calculateRotation(user_generator, XN_SKEL_RIGHT_ELBOW, x, y, z, MM2)) setRotation(getNodeByName("rForeArm"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_RIGHT_HAND, x, y, z, MM2)) setRotation(getNodeByName("rHand"), x, y, z/MM2); // hip -> lThigh -> lShin -> lFoot if (calculateRotation(user_generator, XN_SKEL_RIGHT_HIP, x, y, z, MM3) ) setRotation(getNodeByName("rThigh"), x/MM3, y, z); if (calculateRotation(user_generator, XN_SKEL_RIGHT_KNEE, x, y, z, MM3)) setRotation(getNodeByName("rShin"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_RIGHT_FOOT, x, y, z, MM3)) setRotation(getNodeByName("rFoot"), x/MM3, y, z); // hip -> rThigh -> rShin -> rFoot if (calculateRotation(user_generator, XN_SKEL_LEFT_HIP, x, y, z, MM3)) setRotation(getNodeByName("lThigh"), x/MM3, y, z); if (calculateRotation(user_generator, XN_SKEL_LEFT_KNEE, x, y, z, MM3)) setRotation(getNodeByName("lShin"), x, y, z); if (calculateRotation(user_generator, XN_SKEL_LEFT_FOOT, x, y, z, MM3)) setRotation(getNodeByName("lFoot"), x/MM3, y, z); } // add frame to animation if (is_recording) { setFrame(current_frame++); if (current_frame % 10 == 0) addKeyFrameAllJoints(); else deleteKeyFrameAllJoints(); } }
void start_kinect() { XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; UsersCount = 0; const char *fn = NULL; if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH; else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL; else { printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL); //return XN_STATUS_ERROR; } printf("Reading config from: '%s'\n", fn); nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); //return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); //return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth); CHECK_RC(nRetVal,"No depth"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); CHECK_RC(nRetVal,"No image"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); //return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); //return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); XnUserID aUsers[MAX_NUM_USERS]; XnUInt16 nUsers; XnSkeletonJointTransformation anyjoint; printf("Starting to run\n"); if(g_bNeedPose) { printf("Assume calibration pose\n"); } XnUInt32 epochTime = 0; while (!xnOSWasKeyboardHit()) { g_Context.WaitOneUpdateAll(g_UserGenerator); // print the torso information for the first user already tracking nUsers=MAX_NUM_USERS; g_UserGenerator.GetUsers(aUsers, nUsers); int numTracked=0; int userToPrint=-1; WriteLock w_lock(myLock); pDepthMap = g_depth.GetDepthMap(); pPixelMap = g_image.GetRGB24ImageMap(); g_depth.GetMetaData(g_depthMD); g_image.GetMetaData(g_imageMD); pPixelPoint = g_imageMD.RGB24Data(); for(XnUInt16 i=0; i<nUsers; i++) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE) continue; { /* Writing all new movements into structure*/ /* Head */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_HEAD,anyjoint); Skeletons[i]["Head"]["X"] = anyjoint.position.position.X; Skeletons[i]["Head"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["Head"]["Z"] = anyjoint.position.position.Z; /* Neck */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_NECK,anyjoint); Skeletons[i]["Neck"]["X"] = anyjoint.position.position.X; Skeletons[i]["Neck"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["Neck"]["Z"] = anyjoint.position.position.Z; /* Left Shoulder */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_SHOULDER,anyjoint); Skeletons[i]["LeftShoulder"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftShoulder"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftShoulder"]["Z"] = anyjoint.position.position.Z; /* Right Shoulder */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_SHOULDER,anyjoint); Skeletons[i]["RightShoulder"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightShoulder"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightShoulder"]["Z"] = anyjoint.position.position.Z; /* Torso */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,anyjoint); Skeletons[i]["Torso"]["X"] = anyjoint.position.position.X; Skeletons[i]["Torso"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["Torso"]["Z"] = anyjoint.position.position.Z; /* Left Elbow */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_ELBOW,anyjoint); Skeletons[i]["LeftElbow"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftElbow"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftElbow"]["Z"] = anyjoint.position.position.Z; /* Right Elbow */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_ELBOW,anyjoint); Skeletons[i]["RightElbow"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightElbow"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightElbow"]["Z"] = anyjoint.position.position.Z; /* Left Hip */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HIP,anyjoint); Skeletons[i]["LeftHip"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftHip"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftHip"]["Z"] = anyjoint.position.position.Z; /* Right Hip */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HIP,anyjoint); Skeletons[i]["RightHip"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightHip"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightHip"]["Z"] = anyjoint.position.position.Z; /* Left Hand */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HAND,anyjoint); Skeletons[i]["LeftHand"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftHand"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftHand"]["Z"] = anyjoint.position.position.Z; /* Right Hand */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HAND,anyjoint); Skeletons[i]["RightHand"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightHand"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightHand"]["Z"] = anyjoint.position.position.Z; /* Left Knee */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_KNEE,anyjoint); Skeletons[i]["LeftKnee"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftKnee"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftKnee"]["Z"] = anyjoint.position.position.Z; /* Right Knee */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_KNEE,anyjoint); Skeletons[i]["RightKnee"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightKnee"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightKnee"]["Z"] = anyjoint.position.position.Z; /* Left Foot */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_FOOT,anyjoint); Skeletons[i]["LeftFoot"]["X"] = anyjoint.position.position.X; Skeletons[i]["LeftFoot"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["LeftFoot"]["Z"] = anyjoint.position.position.Z; /* Right Foot */ g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_FOOT,anyjoint); Skeletons[i]["RightFoot"]["X"] = anyjoint.position.position.X; Skeletons[i]["RightFoot"]["Y"] = anyjoint.position.position.Y; Skeletons[i]["RightFoot"]["Z"] = anyjoint.position.position.Z; /*printf("user %d: head at (%6.2f,%6.2f,%6.2f)\n",aUsers[i], Skeletons[i]["Head"]["X"], Skeletons[i]["Head"]["Y"], Skeletons[i]["Head"]["Z"]);*/ } } } g_scriptNode.Release(); g_depth.Release(); g_image.Release(); g_UserGenerator.Release(); g_Context.Release(); }
void sendOSC() { if (handMode) { sendHandOSC(); return; } XnUserID aUsers[15]; XnUInt16 nUsers = 15; userGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { lo_bundle bundle = lo_bundle_new(LO_TT_IMMEDIATE); if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) { oscFunc(&bundle, "head", 0); } if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) { oscFunc(&bundle, "neck", 1); } if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) { oscFunc(&bundle, "l_collar", 2); } if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) { oscFunc(&bundle, "l_shoulder", 3); } if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) { oscFunc(&bundle, "l_elbow", 4); } if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) { oscFunc(&bundle, "l_wrist", 5); } if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) { oscFunc(&bundle, "l_hand", 6); } if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) { oscFunc(&bundle, "l_fingertip", 7); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) { oscFunc(&bundle, "r_collar", 8); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) { oscFunc(&bundle, "r_shoulder", 9); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) { oscFunc(&bundle, "r_elbow", 10); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) { oscFunc(&bundle, "r_wrist", 11); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) { oscFunc(&bundle, "r_hand", 12); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) { oscFunc(&bundle, "r_fingertip", 13); } if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) { oscFunc(&bundle, "torso", 14); } if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) { oscFunc(&bundle, "waist", 15); } if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) { oscFunc(&bundle, "l_hip", 16); } if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) { oscFunc(&bundle, "l_knee", 17); } if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) { oscFunc(&bundle, "l_ankle", 18); } if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0) { oscFunc(&bundle, "l_foot", 19); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0) { oscFunc(&bundle, "r_hip", 20); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0) { oscFunc(&bundle, "r_knee", 21); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0) { oscFunc(&bundle, "r_ankle", 22); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0) { oscFunc(&bundle, "r_foot", 23); } if (lo_send_bundle(addr, bundle) != 0) { printf("error: unable to send bundle\n"); lo_bundle_pp(bundle); } lo_bundle_free_messages(bundle); } else { //Send user's center of mass sendUserPosMsg(aUsers[i]); } } }
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint) { if (!g_UserGenerator.GetSkeletonCap().IsTracking(player)) { printf("not tracked!\n"); return; } if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint)) { return; } XnSkeletonJointPosition joint; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint); if (joint.fConfidence < 0.5) { return; } XnPoint3D pt; pt = joint.position; g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt); drawCircle(pt.X, pt.Y, 2); //////////////////////////////////code///////////////////////////////////////// if(eJoint==XN_SKEL_LEFT_HAND && tmp==0 && player==1){//左手 //printf("ske,%d,0,%f,%f,%f,",player,pt.X,pt.Y,pt.Z); //fflush(stdout); za_tmpX[zaC]=pt.X; za_tmpY[zaC]=pt.Y; za_tmpZ[zaC]=pt.Z; H_L_X=pt.X; H_L_Y=pt.Y; H_L_Z=pt.Z; tmp=1; zaC++; }else if(eJoint==XN_SKEL_RIGHT_HAND && tmp==1 && player==1){//右手 //printf("ske,%d,1,%f,%f,%f,",player,pt.X,pt.Y,pt.Z); za_tmpX[zaC]=pt.X; za_tmpY[zaC]=pt.Y; za_tmpZ[zaC]=pt.Z; H_R_X=pt.X; H_R_Y=pt.Y; H_R_Z=pt.Z; tmp=0; zaC++; LRh=1; //printf("zaC=%d\n",zaC); } //左zaC_MIN<-0,2,4,6,8,10,12,14=zaC_MAX-2 //右zaC_MIN+1<-1,3...15=zaC_MAX-1 //za_tmpなんとか //ズーム判定 //else if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手) if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手) /* if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]); }else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ X=X-(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]); } if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]); }else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ Y=Y-(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]); } */ if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ X=X+(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]); }else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]); } if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ Y=Y+(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]); }else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]); } /* if(X>=10000) X=10000; if(X<=(-10000)) X=(-10000); if(Y>=10000) X=10000; if(Y<=(-10000)) Y=(-10000); */ printf("ske,%d,1,%f,%f,%f,%f,%f,%f,%f,%f,\n",player,za_tmpX[zaC_MAX-2],za_tmpY[zaC_MAX-2],za_tmpZ[zaC_MAX-2],za_tmpX[zaC_MAX-1],za_tmpY[zaC_MAX-1],za_tmpZ[zaC_MAX-1],X,Y); //printf("X=%f,Y=%f\n",X,Y); fflush(stdout); }else if(H_L_Z<kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//両手判定 /* float scaleX_tmp=0; float scaleY_tmp=0; scaleX_tmp=abs((za_tmpX[zaC_MAX-2]-za_tmpX[zaC_MAX-1])/(za_tmpX[zaC_MIN]-za_tmpX[zaC_MIN+1])); scaleY_tmp=abs((za_tmpY[zaC_MAX-2]-za_tmpY[zaC_MAX-1])/(za_tmpY[zaC_MIN]-za_tmpY[zaC_MIN+1])); if(scaleX_tmp>=1) scaleX=scaleX+(scaleX_tmp*0.1); else if(scaleX_tmp<=1) scaleX=scaleX-((1/scaleX_tmp)*0.1); if(scaleY_tmp>=1) scaleY=scaleY+(scaleY_tmp*0.1); else if(scaleY_tmp<=1) scaleY=scaleY-((1/scaleY_tmp)*0.1); if(scaleX<=0.01) scaleX=0.01; if(scaleX>=100) scaleX=100; if(scaleY<=0.01) scaleY=0.01; if(scaleY>=100) scaleY=100; */ float scaleX_tmp=0; float scaleY_tmp=0; float scaleX_tmp2=0; float scaleY_tmp2=0; scaleX_tmp=abs((za_tmpX[zaC_MAX-2]-za_tmpX[zaC_MAX-1])/(za_tmpX[zaC_MIN]-za_tmpX[zaC_MIN+1])); scaleY_tmp=abs((za_tmpY[zaC_MAX-2]-za_tmpY[zaC_MAX-1])/(za_tmpY[zaC_MIN]-za_tmpY[zaC_MIN+1])); //scaleX_tmp=SG( scaleX_tmp2*10.0 )/10.0; //scaleY_tmp=SG( scaleY_tmp2*10.0 )/10.0; if(scaleX_tmp>=1) scaleX=scaleX+(scaleX_tmp*0.01); else if(scaleX_tmp<=1) scaleX=scaleX-((1/scaleX_tmp)*0.01); if(scaleY_tmp>=1) scaleY=scaleY+(scaleY_tmp*0.01); else if(scaleY_tmp<=1) scaleY=scaleY-((1/scaleY_tmp)*0.01); if(scaleX<=0.01) scaleX=0.01; if(scaleX>=100) scaleX=80; if(scaleY<=0.01) scaleY=0.01; if(scaleY>=100) scaleY=80; //printf("scaleX=%f\nscaleY=%f\n",scaleX,scaleY);//テスト printf("ske,%d,2,%f,%f,%f,%f,%f,%f,%f,%f,\n",player,za_tmpX[zaC_MAX-2],za_tmpY[zaC_MAX-2],za_tmpZ[zaC_MAX-2],za_tmpX[zaC_MAX-1],za_tmpY[zaC_MAX-1],za_tmpZ[zaC_MAX-1],scaleX,scaleY); fflush(stdout); }else{ if(LRh==1 && player==1){ printf("ske,%d,0,%f,%f,%f,%f,%f,%f,\n",player,H_L_X,H_L_Y,H_L_Z,H_R_X,H_R_Y,H_R_Z); //printf("Z=%f\n",H_L_Z); fflush(stdout); LRh=0; } } if(zaC==zaC_MAX)zaC=0; //if(LRh==1)LRh=0; fflush(stdout); //////////////////////////////////code///////////////////////////////////////// }
int main(int argc, char **argv) { ros::init(argc, argv, "openni_tracker1"); ros::NodeHandle nh; /* string configFilename = ros::package::getPath("openni_tracker1") + "/openni_tracker1.xml"; XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); CHECK_RC(nRetVal, "InitFromXml"); */ XnStatus nRetVal = g_Context.Init(); CHECK_RC(nRetVal, "Init"); nRetVal = g_Context.OpenFileRecording("/home/latifanjum/Downloads/OpenNI-master/Platform/Linux/Bin/x86-Release/Dataset/Wave2/wave25.oni", g_Player); g_Player.SetRepeat( false ); if (nRetVal != XN_STATUS_OK) { printf("Can't open recording %s\n", xnGetStatusString(nRetVal)); return 1; } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { ROS_INFO("Supplied user generator doesn't support skeleton"); return 1; } XnCallbackHandle hUserCallbacks; g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); XnCallbackHandle hCalibrationCallbacks; g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { ROS_INFO("Pose required, but not supported"); return 1; } XnCallbackHandle hPoseCallbacks; g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); ros::Rate r(30); ros::NodeHandle pnh("~"); string frame_id("openni_depth_frame"); pnh.getParam("camera_frame_id", frame_id); while (ros::ok()) { g_Context.WaitAndUpdateAll(); publishTransforms(frame_id); r.sleep(); } g_Context.Shutdown(); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "PersonTracker"); ros::NodeHandle nh; ros::Rate loop_rate(10); ROS_INFO("Initalizing Arm Connection...."); ros::Publisher leftArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n0/position", 1000); ros::Publisher rightArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n1/position", 1000); yourmom = nh.advertise<prlite_kinematics::some_status_thing>("kinect_hack_status", 1000); ros::Duration(2.0).sleep(); prlite_kinematics::SphereCoordinate coord; prlite_kinematics::some_status_thing status; coord.radius = 35.0; coord.phi = 0.0; coord.theta = 0.0; status.lulz = 0; //initialized/reset yourmom.publish(status); leftArmPublisher.publish(coord); rightArmPublisher.publish(coord); ROS_INFO("Initalizing Kinect + NITE...."); XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); ROS_INFO("Ready To Go!\n"); while (ros::ok()) { // Update OpenNI g_Context.WaitAndUpdateAll(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); // Print positions XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // Read joint positions for person (No WRISTs) XnSkeletonJointPosition torsoPosition, lShoulderPosition, rShoulderPosition, neckPosition, headPosition, lElbowPosition, rElbowPosition; XnSkeletonJointPosition rWristPosition, lWristPosition, rHipPosition, lHipPosition, lKneePosition, rKneePosition; XnSkeletonJointPosition lFootPosition, rFootPosition; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_TORSO, torsoPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_NECK, neckPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, headPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, lWristPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, rWristPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HIP, lHipPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HIP, rHipPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_KNEE, lKneePosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneePosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_FOOT, lFootPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootPosition); // Read Joint Orientations XnSkeletonJointOrientation torsoOrientation, lShoulderOrientation, rShoulderOrientation, lHipOrientation, rHipOrientation; XnSkeletonJointOrientation lWristOrientation, rWristOrientation, lElbowOrientation, rElbowOrientation; XnSkeletonJointOrientation headOrientation, neckOrientation; XnSkeletonJointOrientation lKneeOrientation, rKneeOrientation, lFootOrientation, rFootOrientation; g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_TORSO, torsoOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_HEAD, headOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_NECK, neckOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HIP, lHipOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HIP, rHipOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HAND, lWristOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HAND, rWristOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_KNEE, lKneeOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneeOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_FOOT, lFootOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootOrientation); XnFloat* m = torsoOrientation.orientation.elements; KDL::Rotation torsoO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lShoulderOrientation.orientation.elements; KDL::Rotation lShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rShoulderOrientation.orientation.elements; KDL::Rotation rShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lHipOrientation.orientation.elements; KDL::Rotation lHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rHipOrientation.orientation.elements; KDL::Rotation rHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = headOrientation.orientation.elements; KDL::Rotation headO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = neckOrientation.orientation.elements; KDL::Rotation neckO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lElbowOrientation.orientation.elements; KDL::Rotation lElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rElbowOrientation.orientation.elements; KDL::Rotation rElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lWristOrientation.orientation.elements; KDL::Rotation lWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rWristOrientation.orientation.elements; KDL::Rotation rWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lKneeOrientation.orientation.elements; KDL::Rotation lKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rKneeOrientation.orientation.elements; KDL::Rotation rKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lFootOrientation.orientation.elements; KDL::Rotation lFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rFootOrientation.orientation.elements; KDL::Rotation rFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); double qx = 0.0, qy = 0.0, qz = 0.0, qw = 0.0; // Get Points in 3D space XnPoint3D torso = torsoPosition.position; XnPoint3D lShould = lShoulderPosition.position; XnPoint3D rShould = rShoulderPosition.position; XnPoint3D lElbow = lElbowPosition.position; XnPoint3D rElbow = rElbowPosition.position; XnPoint3D lWrist = lWristPosition.position; XnPoint3D rWrist = rWristPosition.position; XnPoint3D neck = neckPosition.position; XnPoint3D head = headPosition.position; XnPoint3D lHip = lHipPosition.position; XnPoint3D rHip = rHipPosition.position; XnPoint3D lKnee = lKneePosition.position; XnPoint3D rKnee = rKneePosition.position; XnPoint3D lFoot = lFootPosition.position; XnPoint3D rFoot = rFootPosition.position; // ---------- ARM CONTROLLER HACK ------------------ // Calculate arm length of user double lenL = calc3DDist( lShould, lElbow ) + calc3DDist( lElbow, lWrist ); double lenR = calc3DDist( rShould, rElbow ) + calc3DDist( rElbow, rWrist ); double distL = calc3DDist(lShould, lWrist); double distR = calc3DDist(rShould, rWrist); // Calculate positions prlite_kinematics::SphereCoordinate lCoord; prlite_kinematics::SphereCoordinate rCoord; lCoord.radius = 35 * (distL / lenL); rCoord.radius = 35 * (distR / lenR); lCoord.theta = -atan2(lShould.X - lWrist.X, lShould.Z - lWrist.Z); rCoord.theta = -atan2(rShould.X - rWrist.X, rShould.Z - rWrist.Z); if(lCoord.theta > 1.0) lCoord.theta = 1.0; if(lCoord.theta < -1.0) lCoord.theta = -1.0; if(rCoord.theta > 1.0) rCoord.theta = 1.0; if(rCoord.theta < -1.0) rCoord.theta = -1.0; lCoord.phi = -atan2(lShould.Y - lWrist.Y, lShould.X - lWrist.X); rCoord.phi = -atan2(rShould.Y - rWrist.Y, rShould.X - rWrist.X); if(lCoord.phi > 1.25) lCoord.phi = 1.25; if(lCoord.phi < -0.33) lCoord.phi = -0.33; if(rCoord.phi > 1.2) rCoord.phi = 1.25; if(rCoord.phi < -0.33) rCoord.phi = -0.33; ROS_INFO("User %d: Left (%lf,%lf,%lf), Right (%lf,%lf,%lf)", i, lCoord.radius, lCoord.theta, lCoord.phi, rCoord.radius, rCoord.theta, rCoord.phi); // Publish to arms leftArmPublisher.publish(rCoord); rightArmPublisher.publish(lCoord); // ---------- END HACK ----------------- // Publish Transform static tf::TransformBroadcaster br; tf::Transform transform; std::stringstream body_name, neck_name, lshould_name, rshould_name, head_name, relbow_name, lelbow_name, lwrist_name, rwrist_name; std::stringstream lhip_name, rhip_name, lknee_name, rknee_name, lfoot_name, rfoot_name; body_name << "Torso (" << i << ")" << std::ends; neck_name << "Neck (" << i << ")" << std::ends; head_name << "Head (" << i << ")" << std::ends; lshould_name << "Shoulder L (" << i << ")" << std::ends; rshould_name << "Shoulder R (" << i << ")" << std::ends; lelbow_name << "Elbow L (" << i << ")" << std::ends; relbow_name << "Elbow R (" << i << ")" << std::ends; lwrist_name << "Wrist L (" << i << ")" << std::ends; rwrist_name << "Wrist R (" << i << ")" << std::ends; lhip_name << "Hip L (" << i << ")" << std::ends; rhip_name << "Hip R (" << i << ")" << std::ends; lknee_name << "Knee L (" << i << ")" << std::ends; rknee_name << "Knee R (" << i << ")" << std::ends; lfoot_name << "Foot L (" << i << ")" << std::ends; rfoot_name << "Foot R (" << i << ")" << std::ends; // Publish Torso torsoO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(torso.X / ADJUST_VALUE, torso.Y / ADJUST_VALUE, torso.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", body_name.str().c_str())); // Publish Left Shoulder lShouldO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lShould.X / ADJUST_VALUE, lShould.Y / ADJUST_VALUE, lShould.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lshould_name.str().c_str())); // Publish Right Shoulder rShouldO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rShould.X / ADJUST_VALUE, rShould.Y / ADJUST_VALUE, rShould.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rshould_name.str().c_str())); // Publish Left Elbow lElbowO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lElbow.X / ADJUST_VALUE, lElbow.Y / ADJUST_VALUE, lElbow.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lelbow_name.str().c_str())); // Publish Right Elbow rElbowO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rElbow.X / ADJUST_VALUE, rElbow.Y / ADJUST_VALUE, rElbow.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", relbow_name.str().c_str())); // Publish Left Wrist lWristO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lWrist.X / ADJUST_VALUE, lWrist.Y / ADJUST_VALUE, lWrist.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lwrist_name.str().c_str())); // Publish Right Wrist rWristO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rWrist.X / ADJUST_VALUE, rWrist.Y / ADJUST_VALUE, rWrist.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rwrist_name.str().c_str())); // Publish Neck neckO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(neck.X / ADJUST_VALUE, neck.Y / ADJUST_VALUE, neck.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", neck_name.str().c_str())); // Publish Head headO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(head.X / ADJUST_VALUE, head.Y / ADJUST_VALUE, head.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", head_name.str().c_str())); // Publish Left Hip lHipO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lHip.X / ADJUST_VALUE, lHip.Y / ADJUST_VALUE, lHip.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lhip_name.str().c_str())); // Publish Right Hip rHipO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rHip.X / ADJUST_VALUE, rHip.Y / ADJUST_VALUE, rHip.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rhip_name.str().c_str())); // Publish Left Knee lKneeO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lKnee.X / ADJUST_VALUE, lKnee.Y / ADJUST_VALUE, lKnee.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lknee_name.str().c_str())); // Publish Right Knee rKneeO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rKnee.X / ADJUST_VALUE, rKnee.Y / ADJUST_VALUE, rKnee.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rknee_name.str().c_str())); // Publish Left Foot lFootO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lFoot.X / ADJUST_VALUE, lFoot.Y / ADJUST_VALUE, lFoot.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lfoot_name.str().c_str())); // Publish Right Foot rFootO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rFoot.X / ADJUST_VALUE, rFoot.Y / ADJUST_VALUE, rFoot.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rfoot_name.str().c_str())); } } ros::spinOnce(); //loop_rate.sleep(); } g_Context.Shutdown(); return 0; }
void initOpenNi() { XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); } else { printf("Kinect Config loaded: %s\n", xnGetStatusString(nRetVal)); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); if (nRetVal != XN_STATUS_OK) { printf("No depth generator found. Using a default one..."); xn::MockDepthGenerator mockDepth; nRetVal = mockDepth.Create(g_Context); // set some defaults XnMapOutputMode defaultMode; //resolution defaultMode.nXRes = 640; defaultMode.nYRes = 480; //fps defaultMode.nFPS = 30; nRetVal = mockDepth.SetMapOutputMode(defaultMode); // set FOV XnFieldOfView fov; fov.fHFOV = 1.0225999419141749; fov.fVFOV = 0.79661567681716894; nRetVal = mockDepth.SetGeneralProperty(XN_PROP_FIELD_OF_VIEW, sizeof(fov), &fov); XnUInt32 nDataSize = defaultMode.nXRes * defaultMode.nYRes * sizeof(XnDepthPixel); XnDepthPixel* pData = (XnDepthPixel*)xnOSCallocAligned(nDataSize, 1, XN_DEFAULT_MEM_ALIGN); nRetVal = mockDepth.SetData(1, 0, nDataSize, pData); g_DepthGenerator = mockDepth; } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); } std::cout << "Usergenerator created: " << xnGetStatusString(nRetVal) << std::endl; XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); //g_UserGenerator.GetSkeletonCap().SetSmoothing(0.90f); nRetVal = g_Context.StartGeneratingAll(); if (nRetVal != XN_STATUS_OK) { printf("Unable to start Generating."); } else { std::cout << "Start generating: " << xnGetStatusString(nRetVal) << std::endl; } std::cout << "OpenNi init successful" << std::endl; }
float LArm::getElbowYaw(XnUserID user, xn::UserGenerator guser) { XnSkeletonJointTransformation shoulder, elbow, hand, pastShoulder, pastElbow, pastHand; LArm varAngle, refAx; XnReferenceAxis elbowRefAx; XnVector3D elToHand, proyVectorZ, pImag, proyVectorPlane, pProyOverXY, pOverAxY; float changeAngle, finalAngle; pastShoulder.position.position = shoulder.position.position; pastHand.position.position = hand.position.position; pastElbow.position.position = elbow.position.position; guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_SHOULDER,shoulder); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_ELBOW,elbow); guser.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_HAND,hand); ///Marco de referencia en un momento anterior con punto central sobre el codo elbowRefAx = refAx.generateReference(pastHand.position.position, pastElbow.position.position, pastShoulder.position.position); ///Obtenemos el vector elToHand (codo -> mano) elToHand.X = hand.position.position.X-elbow.position.position.X; elToHand.Y = hand.position.position.Y-elbow.position.position.Y; elToHand.Z = hand.position.position.Z-elbow.position.position.Z; ///Obtenemos la proyeccion del vector elToHand sobre el eje Z de la referencia en el codo proyVectorZ = getProyectionVector(elToHand, elbowRefAx.NewZ); ///Obtenemos el punto imaginario pImag.X = proyVectorZ.X + elbow.position.position.X; pImag.Y = proyVectorZ.Y + elbow.position.position.Y; pImag.Z = proyVectorZ.Z + elbow.position.position.Z; ///Obtenemos la proyeccion sobre el plano XY, para ello obtenemos se forma entre la proyeccion y el vector elToHand proyVectorPlane.X = hand.position.position.X - pImag.X; proyVectorPlane.Y = hand.position.position.Y - pImag.Y; proyVectorPlane.Z = hand.position.position.Z - pImag.Z; ///Obtenemos primer punto que es la proyeccion de la mano sobre el plano XY pProyOverXY.X = elbow.position.position.X + proyVectorPlane.X; pProyOverXY.Y = elbow.position.position.Y + proyVectorPlane.Y; pProyOverXY.Z = elbow.position.position.Z + proyVectorPlane.Z; ///Obtenemos el segundo punto sobre nuestro eje Y pOverAxY.X = elbow.position.position.X + elbowRefAx.NewY.X; pOverAxY.Y = elbow.position.position.Y + elbowRefAx.NewY.Y; pOverAxY.Z = elbow.position.position.Z + elbowRefAx.NewY.Z; ///Obtenemos el angulo entre la proyeccion entre el plano XY y el eje Y changeAngle = varAngle.getAngle(pProyOverXY, elbow.position.position, pOverAxY); ///Determinamos el signo del angulo para saber si rotamos el codo en sentido horario o antihorario ///Obtenemos la proyeccion del vector proyVectorPlane sobre el eje X de la referencia en el codo if(getProyection(proyVectorPlane,elbowRefAx.NewX) < 0){ finalAngle = finalAngle+changeAngle; } else{ finalAngle = finalAngle-changeAngle; } return(finalAngle); }
/// execution loop: runs the hole time: main logic void executeInputLoop() { PackedType<Vector3<double> > packedVector3; //get the new informations and sent it over the udp socket to the tui server while (true) { /// wait for new kinect data g_Context.WaitOneUpdateAll(g_UserGenerator); /// max 15 users XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); /// track only the closest person //todo: hau das in eine einzelne function!! std::vector<XnPoint3D> userCoM(nUsers); for (int i = 0; i < nUsers; ++i) { g_UserGenerator.GetCoM(aUsers[i], userCoM[i]); } //calculate the user with the minimum distance to the kinect float minDist = FLT_MAX; int minUser = -1; float tmp_minDist = 0.0f; for (int i = 0; i < nUsers; ++i){ if (userCoM[i].X + userCoM[i].Y + userCoM[i].Z == 0) { continue; } tmp_minDist = userCoM[i].X*userCoM[i].X + userCoM[i].Y*userCoM[i].Y + userCoM[i].Z*userCoM[i].Z; if(tmp_minDist < minDist && tmp_minDist > 0.0){ minDist = tmp_minDist; minUser = i; } //std::cout << "user: "******"sending user: " << aUsers[minUser] << std::endl; vector<pair<int, Vector3<double> > > & v = packedVector3.getItems(); v.clear(); addJoint(aUsers[minUser], XN_SKEL_HEAD , SKEL_HEAD , packedVector3); addJoint(aUsers[minUser], XN_SKEL_NECK , SKEL_NECK , packedVector3); addJoint(aUsers[minUser], XN_SKEL_TORSO , SKEL_TORSO , packedVector3); addJoint(aUsers[minUser], XN_SKEL_WAIST , SKEL_WAIST , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_COLLAR , SKEL_LEFT_COLLAR , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_SHOULDER , SKEL_LEFT_SHOULDER , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_ELBOW , SKEL_LEFT_ELBOW , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_WRIST , SKEL_LEFT_WRIST , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_HAND , SKEL_LEFT_HAND , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_FINGERTIP , SKEL_LEFT_FINGERTIP , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_COLLAR , SKEL_RIGHT_COLLAR , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_SHOULDER , SKEL_RIGHT_SHOULDER , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_ELBOW , SKEL_RIGHT_ELBOW , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_WRIST , SKEL_RIGHT_WRIST , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_HAND , SKEL_RIGHT_HAND , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_FINGERTIP, SKEL_RIGHT_FINGERTIP, packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_HIP , SKEL_LEFT_HIP , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_KNEE , SKEL_LEFT_KNEE , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_ANKLE , SKEL_LEFT_ANKLE , packedVector3); addJoint(aUsers[minUser], XN_SKEL_LEFT_FOOT , SKEL_LEFT_FOOT , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_HIP , SKEL_RIGHT_HIP , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_KNEE , SKEL_RIGHT_KNEE , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_ANKLE , SKEL_RIGHT_ANKLE , packedVector3); addJoint(aUsers[minUser], XN_SKEL_RIGHT_FOOT , SKEL_RIGHT_FOOT , packedVector3); sendPackedVector3(packedVector3); } //if we use openGL, than we have to break up after one send //s.t. openGL can draw a new scene if(enableOpenGL) { break; } else { Sleep(10); } } }
void XN_CALLBACK_TYPE user_exit(xn::UserGenerator& generator, XnUserID nId, void *pCookie) { printf("{\"user_exit\": {\"userid\": %d}}\n", nId); userGenerator.GetSkeletonCap().StopTracking(nId); userGenerator.GetSkeletonCap().Reset(nId); }
int main(int argc, char **argv) { sleep(10); ros::init(argc, argv, "skel_tracker"); ros::NodeHandle nh_; // Read the device_id parameter from the server int device_id; // param_nh.param ("device_id", device_id, argc > 1 ? atoi (argv[1]) : 0); pmap_pub = nh_.advertise<mapping_msgs::PolygonalMap> ("skeletonpmaps", 100); skel_pub = nh_.advertise<body_msgs::Skeletons> ("skeletons", 100); XnStatus nRetVal = XN_STATUS_OK; if (argc > 1) { nRetVal = g_Context.Init(); CHECK_RC(nRetVal, "Init"); nRetVal = g_Context.OpenFileRecording(argv[1]); if (nRetVal != XN_STATUS_OK) { printf("Can't open recording %s: %s\n", argv[1], xnGetStatusString(nRetVal)); return 1; } } else { std::string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml"; nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); CHECK_RC(nRetVal, "InitFromXml"); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); glInit(&argc, argv); glutMainLoop(); }
// Callback: Detected a pose void XN_CALLBACK_TYPE pose_detected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie) { // printf("{'pose_detected\":{\"userid\":%d,'type':'%s'}\n", nId, strPose); userGenerator.GetPoseDetectionCap().StopPoseDetection(nId); userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); }
void getSkeletonJoint(XnUserID player, body_msgs::SkeletonJoint &j, XnSkeletonJoint name){ XnSkeletonJointPosition joint1; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, name, joint1); j.position= vecToPt(joint1.position); j.confidence = joint1.fConfidence; }
void XN_CALLBACK_TYPE PoseDetected(xn::PoseDetectionCapability& pose, const XnChar* strPose, XnUserID user, void* cxt) { printf("Found pose \"%s\" for user %d\n", strPose, user); g_UserGenerator.GetSkeletonCap().RequestCalibration(user, TRUE); g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(user); }
int getJoints(XnUserID user){ //Locals XnSkeletonJointTransformation joint; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_TORSO,joint); if(joint.position.fConfidence > .8){ jointArr[0] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_HEAD,joint); if(joint.position.fConfidence > .8){ jointArr[1] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_NECK,joint); if(joint.position.fConfidence > .8){ jointArr[2] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_SHOULDER,joint); if(joint.position.fConfidence > .8){ jointArr[3] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_SHOULDER,joint); if(joint.position.fConfidence > .8){ jointArr[4] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_ELBOW,joint); if(joint.position.fConfidence > .8){ jointArr[5] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_ELBOW,joint); if(joint.position.fConfidence > .8){ jointArr[6] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_HAND,joint); if(joint.position.fConfidence > .8){ jointArr[7] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_HAND,joint); if(joint.position.fConfidence > .8){ jointArr[8] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_RIGHT_HIP,joint); if(joint.position.fConfidence > .8){ jointArr[9] = joint; } else return 0; g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(user,XN_SKEL_LEFT_HIP,joint); if(joint.position.fConfidence > .8){ jointArr[10] = joint; } else return 0; //The function succeeded return 1; }
// this function will calculate the average joint confidence value for every user float jointConfidence(XnUserID nId) { int userSum = 0; // holds the sum of all the joint confidence values float userConfidence; // holds average of sum of all joint confidence values // ITERATE THROUGH EACH JOINT AND RECORD CONFIDENCE VALUES if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(nId)){ XnSkeletonJointPosition jointData; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_HEAD, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_NECK, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_TORSO, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_LEFT_SHOULDER, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_LEFT_ELBOW, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_LEFT_HAND, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_RIGHT_SHOULDER, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_RIGHT_ELBOW, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_RIGHT_HAND, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_LEFT_HIP, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_LEFT_KNEE, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_LEFT_FOOT, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_RIGHT_HIP, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_RIGHT_KNEE, jointData); userSum += jointData.fConfidence; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId, XN_SKEL_RIGHT_FOOT, jointData); userSum += jointData.fConfidence; userConfidence = userSum / 15.0f; // sum of confidence values divided by the fifteen joints return userConfidence; } else std::cout << "User " << nId << " is not calibrated!" << std::endl; }
int main(int argc, char **argv) { commInit("192.168.1.255", 54321); XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; const char *fn = NULL; if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH; else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL; else { printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL); return XN_STATUS_ERROR; } printf("Reading config from: '%s'\n", fn); nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal,"No depth"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); XnUserID aUsers[MAX_NUM_USERS]; XnUInt16 nUsers; int j; printf("Starting to run\n"); if(g_bNeedPose) { printf("Assume calibration pose\n"); } XnUInt32 epochTime = 0; while (!xnOSWasKeyboardHit()) { g_Context.WaitOneUpdateAll(g_UserGenerator); // print the torso information for the first user already tracking nUsers=MAX_NUM_USERS; g_UserGenerator.GetUsers(aUsers, nUsers); int numTracked=0; int userToPrint=-1; for(XnUInt16 i=0; i<nUsers; i++) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE) continue; if(getJoints(aUsers[i])){ /** printf("Left Elbow: %.2f\nLeft Shoulder Roll: %.2f\nLeft Shoulder Pitch: %.2f\nHead Pitch: %.2f\n", findAngle(jointArr[6], jointArr[4], jointArr[8], 0), findAngle(jointArr[4], jointArr[10], jointArr[6], 0), findAngle(jointArr[4], jointArr[10], jointArr[6], 1), findAngle(jointArr[2], jointArr[1], jointArr[0], 1) ); **/ float headAngle = findAngle(jointArr[2], jointArr[1], jointArr[0], 1); float leftElbowAngle = findAngle(jointArr[6], jointArr[4], jointArr[8], 0); float leftShoulderRoll = findAngle(jointArr[4], jointArr[10], jointArr[6], 0); float leftShoulderPitch = findAngle(jointArr[4], jointArr[10], jointArr[6], 1); float rightElbowAngle = findAngle(jointArr[5], jointArr[3], jointArr[7], 0); float rightShoulderRoll = findAngle(jointArr[3], jointArr[9], jointArr[5], 0); float rightShoulderPitch = findAngle(jointArr[3], jointArr[9], jointArr[5], 1); ostringstream ostr; ostr << "{"<<headAngle<<",{"<<leftShoulderPitch<<","<<leftShoulderRoll<<","<< leftElbowAngle<<"},{"<<rightShoulderPitch<<","<<rightShoulderRoll<<","<<rightElbowAngle<<"},}"; string send = ostr.str(); //cout<<send; //printRotation(aUsers[i]); commSend(send); usleep(200000); } } } g_scriptNode.Release(); g_DepthGenerator.Release(); g_UserGenerator.Release(); g_Context.Release(); }
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd) { static bool bInitialized = false; static GLuint depthTexID; static unsigned char* pDepthTexBuf; static int texWidth, texHeight; float topLeftX; float topLeftY; float bottomRightY; float bottomRightX; float texXpos; float texYpos; if(!bInitialized) { texWidth = getClosestPowerOfTwo(dmd.XRes()); texHeight = getClosestPowerOfTwo(dmd.YRes()); // printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight); depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ; // printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight); bInitialized = true; topLeftX = dmd.XRes(); topLeftY = 0; bottomRightY = dmd.YRes(); bottomRightX = 0; texXpos =(float)dmd.XRes()/texWidth; texYpos =(float)dmd.YRes()/texHeight; memset(texcoords, 0, 8*sizeof(float)); texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos; } unsigned int nValue = 0; unsigned int nHistValue = 0; unsigned int nIndex = 0; unsigned int nX = 0; unsigned int nY = 0; unsigned int nNumberOfPoints = 0; XnUInt16 g_nXRes = dmd.XRes(); XnUInt16 g_nYRes = dmd.YRes(); unsigned char* pDestImage = pDepthTexBuf; const XnDepthPixel* pDepth = dmd.Data(); const XnLabel* pLabels = smd.Data(); // Calculate the accumulative histogram memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float)); for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX<g_nXRes; nX++) { nValue = *pDepth; if (nValue != 0) { g_pDepthHist[nValue]++; nNumberOfPoints++; } pDepth++; } } for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1]; } if (nNumberOfPoints) { for (nIndex=1; nIndex<MAX_DEPTH; nIndex++) { g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints))); } } pDepth = dmd.Data(); if (g_bDrawPixels) { XnUInt32 nIndex = 0; // Prepare the texture map for (nY=0; nY<g_nYRes; nY++) { for (nX=0; nX < g_nXRes; nX++, nIndex++) { pDestImage[0] = 0; pDestImage[1] = 0; pDestImage[2] = 0; if (g_bDrawBackground || *pLabels != 0) { nValue = *pDepth; XnLabel label = *pLabels; XnUInt32 nColorID = label % nColors; if (label == 0) { nColorID = nColors; } if (nValue != 0) { nHistValue = g_pDepthHist[nValue]; pDestImage[0] = nHistValue * Colors[nColorID][0]; pDestImage[1] = nHistValue * Colors[nColorID][1]; pDestImage[2] = nHistValue * Colors[nColorID][2]; } } pDepth++; pLabels++; pDestImage+=3; } pDestImage += (texWidth - g_nXRes) *3; } } else { xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes); } glBindTexture(GL_TEXTURE_2D, depthTexID); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf); // Display the OpenGL texture map glColor4f(0.75,0.75,0.75,1); glEnable(GL_TEXTURE_2D); DrawTexture(dmd.XRes(),dmd.YRes(),0,0); glDisable(GL_TEXTURE_2D); char strLabel[50] = ""; XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { #ifndef USE_GLES if (g_bPrintID) { XnPoint3D com; g_UserGenerator.GetCoM(aUsers[i], com); g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com); xnOSMemSet(strLabel, 0, sizeof(strLabel)); if (!g_bPrintState) { // Tracking sprintf(strLabel, "%d", aUsers[i]); } else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // Tracking sprintf(strLabel, "%d - Tracking", aUsers[i]); } else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i])) { // Calibrating sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first)); } else { // Nothing sprintf(strLabel, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second)); } glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1); glRasterPos2i(com.X, com.Y); glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); } #endif if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { #ifndef USE_GLES glBegin(GL_LINES); #endif glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1); DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK); DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER); DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW); DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND); DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER); DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW); DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND); DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO); DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO); DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP); DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE); DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT); DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP); DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE); DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT); DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP); #ifndef USE_GLES glEnd(); #endif } } }
// Callback: Detected a pose void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie) { printf("Pose %s detected for user %d\n", strPose, nId); g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId); g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE); }
int main(int argc, char **argv) { XnStatus nRetVal = XN_STATUS_OK; if (argc > 1) { nRetVal = g_Context.Init(); CHECK_RC(nRetVal, "Init"); nRetVal = g_Context.OpenFileRecording(argv[1], g_Player); if (nRetVal != XN_STATUS_OK) { printf("Can't open recording %s: %s\n", argv[1], xnGetStatusString(nRetVal)); return 1; } } else { xn::EnumerationErrors errors; nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, g_scriptNode, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); if (nRetVal != XN_STATUS_OK) { printf("No depth generator found. Using a default one..."); xn::MockDepthGenerator mockDepth; nRetVal = mockDepth.Create(g_Context); CHECK_RC(nRetVal, "Create mock depth"); // set some defaults XnMapOutputMode defaultMode; defaultMode.nXRes = 320; defaultMode.nYRes = 240; defaultMode.nFPS = 30; nRetVal = mockDepth.SetMapOutputMode(defaultMode); CHECK_RC(nRetVal, "set default mode"); // set FOV XnFieldOfView fov; fov.fHFOV = 1.0225999419141749; fov.fVFOV = 0.79661567681716894; nRetVal = mockDepth.SetGeneralProperty(XN_PROP_FIELD_OF_VIEW, sizeof(fov), &fov); CHECK_RC(nRetVal, "set FOV"); XnUInt32 nDataSize = defaultMode.nXRes * defaultMode.nYRes * sizeof(XnDepthPixel); XnDepthPixel* pData = (XnDepthPixel*)xnOSCallocAligned(nDataSize, 1, XN_DEFAULT_MEM_ALIGN); nRetVal = mockDepth.SetData(1, 0, nDataSize, pData); CHECK_RC(nRetVal, "set empty depth map"); g_DepthGenerator = mockDepth; } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected, hCalibrationInProgress, hPoseInProgress; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); CHECK_RC(nRetVal, "Register to user callbacks"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart); CHECK_RC(nRetVal, "Register to calibration start"); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete); CHECK_RC(nRetVal, "Register to calibration complete"); XnCallbackHandle hUserExitCB, hUserReenterCB; nRetVal = g_UserGenerator.RegisterToUserExit(User_Exit, NULL, hUserExitCB); CHECK_RC(nRetVal, "Register to user exit"); nRetVal = g_UserGenerator.RegisterToUserReEnter(User_ReEnter, NULL, hUserReenterCB); CHECK_RC(nRetVal, "Register to user re-enter"); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected); CHECK_RC(nRetVal, "Register to Pose Detected"); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseInProgress(MyPoseInProgress, NULL, hPoseInProgress); CHECK_RC(nRetVal, "Register to pose in progress"); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationInProgress(MyCalibrationInProgress, NULL, hCalibrationInProgress); CHECK_RC(nRetVal, "Register to calibration in progress"); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); #ifndef USE_GLES glInit(&argc, argv); glutMainLoop(); #else if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context)) { printf("Error initializing opengles\n"); CleanupExit(); } glDisable(GL_DEPTH_TEST); glEnable(GL_TEXTURE_2D); glEnableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_COLOR_ARRAY); while (!g_bQuit) { glutDisplay(); eglSwapBuffers(display, surface); } opengles_shutdown(display, surface, context); CleanupExit(); #endif }
void sendOSC() { if (handMode) { sendHandOSC(); return; } XnUserID aUsers[15]; XnUInt16 nUsers = 15; userGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { osc::OutboundPacketStream p(osc_buffer, OUTPUT_BUFFER_SIZE); p << osc::BeginBundleImmediate; if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) { oscFunc(&p, "head"); } if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) { oscFunc(&p, "neck"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) { oscFunc(&p, "l_collar"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) { oscFunc(&p, "l_shoulder"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) { oscFunc(&p, "l_elbow"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) { oscFunc(&p, "l_wrist"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) { oscFunc(&p, "l_hand"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) { oscFunc(&p, "l_fingertip"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) { oscFunc(&p, "r_collar"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) { oscFunc(&p, "r_shoulder"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) { oscFunc(&p, "r_elbow"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) { oscFunc(&p, "r_wrist"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) { oscFunc(&p, "r_hand"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) { oscFunc(&p, "r_fingertip"); } if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) { oscFunc(&p, "torso"); } if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) { oscFunc(&p, "waist"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) { oscFunc(&p, "l_hip"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) { oscFunc(&p, "l_knee"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) { oscFunc(&p, "l_ankle"); } if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0) { oscFunc(&p, "l_foot"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0) { oscFunc(&p, "r_hip"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0) { oscFunc(&p, "r_knee"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0) { oscFunc(&p, "r_ankle"); } if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0) { oscFunc(&p, "r_foot"); } p << osc::EndBundle; transmitSocket->Send(p.Data(), p.Size()); } else { //Send user's center of mass sendUserPosMsg(aUsers[i]); } } }