// this function is called each frame void glutDisplay (void) { glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); glDisable(GL_TEXTURE_2D); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } // Process the data g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); DrawDepthMap(depthMD, sceneMD); glutSwapBuffers(); Sleep(5); // todo: adjust }
bool CvCapture_OpenNI::grabFrame() { if( !isOpened() ) return false; bool isGrabbed = false; if( !approxSyncGrabber.empty() && approxSyncGrabber->isRun() ) { isGrabbed = approxSyncGrabber->grab( depthMetaData, imageMetaData ); } else { XnStatus status = context.WaitAndUpdateAll(); if( status != XN_STATUS_OK ) return false; if( depthGenerator.IsValid() ) depthGenerator.GetMetaData( depthMetaData ); if( imageGenerator.IsValid() ) imageGenerator.GetMetaData( imageMetaData ); isGrabbed = true; } return isGrabbed; }
// this function is called each frame void glutDisplay (void) { // Read next available data g_Context.WaitAndUpdateAll(); // Process the data g_pSessionManager->Update(&g_Context); glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); #ifdef USE_GLUT glOrtho(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0); #else glOrthof(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0); #endif glDisable(GL_TEXTURE_2D); // Draw the MyBoxes g_pBox[0]->Draw(); g_pBox[1]->Draw(); g_pBox[2]->Draw(); // Draw the slider DrawSlider(); #ifdef USE_GLUT glutSwapBuffers(); #endif }
void Loop(int sock) { XnStatus nRetVal = XN_STATUS_OK; struct timespec last,now; double nowtime, starttime; clock_gettime(CLOCK_REALTIME, &last); double lasttime = (double)(last.tv_sec) + ((double)(last.tv_nsec))/1000000000; int frames=0; while (g_notDone) { if ((nRetVal = g_context.WaitAndUpdateAll()) != XN_STATUS_OK) { fprintf(stderr,"Could not update ir: %s\n", xnGetStatusString(nRetVal)); continue; } const XnDepthPixel* pDepthMap = g_depth.GetDepthMap(); const XnRGB24Pixel* pImage = NULL;//g_image.GetRGB24ImageMap(); const XnIRPixel* pIR = NULL;//g_ir.GetIRMap(); ProcessDepthFrame(pDepthMap, g_depthWidth, g_depthHeight); FindFingertip(); frames++; clock_gettime(CLOCK_REALTIME, &now); nowtime = (double)(now.tv_sec) + ((double)(now.tv_nsec))/1000000000; if (g_stabalize) // If we are still stablizing then don't do anything { if ((nowtime - starttime) >= STABILIZATION_TIME_SECS) { g_stabalize = FALSE; g_set_background = TRUE; } } else if (g_calibrate) // Do we need to calibrate? Calibrate(sock); else SendFingers(sock); // otherwise just send the touches /* if (nowtime - lasttime >= 1.0) { printf("%d FPS\n", (int)(frames/(nowtime-lasttime))); lasttime = nowtime; frames = 0; if (sock >= 0 && pDepthMap != 0 )// && pImage != 0 )//&& pIR != 0) SendImage(sock,pDepthMap, pImage, pIR, g_depthWidth, g_depthHeight); } */ } }
/* DWORD WINAPI EEThreadProc(LPVOID lpThreadParameter) { { g_Context.WaitAndUpdateAll(); if (g_bRecord) { g_pRecorder->Record(); } } return 0; } */ int EEThreadProc(void *lpThreadParameter) { { g_Context.WaitAndUpdateAll(); if (g_bRecord) { g_pRecorder->Record(); } } return 0; }
// メインループ void run() { int nAudioNextBuffer = 0; printf ("Press any key to exit...\n"); // 今のデータを捨てる audio.WaitAndUpdateData(); while (!xnOSWasKeyboardHit()) { // データの更新 XnStatus nRetVal = context.WaitAndUpdateAll(); if (nRetVal != XN_STATUS_OK) { throw std::runtime_error(xnGetStatusString(nRetVal)); } // バッファの取得 WAVEHDR* pHeader = &AudioBuffers[nAudioNextBuffer]; if ((pHeader->dwFlags & WHDR_DONE) == 0) { printf("No audio buffer is available!. Audio buffer will be lost!\n"); continue; } // WAVEヘッダのクリーンアップ MMRESULT mmRes = waveOutUnprepareHeader(hWaveOut, pHeader, sizeof(WAVEHDR)); if ( mmRes != MMSYSERR_NOERROR ) { OutputErrorText( mmRes ); } // WAVEデータの取得 pHeader->dwBufferLength = audio.GetDataSize(); pHeader->dwFlags = 0; xnOSMemCopy(pHeader->lpData, audio.GetAudioBuffer(), pHeader->dwBufferLength); // WAVEヘッダの初期化 mmRes = waveOutPrepareHeader(hWaveOut, pHeader, sizeof(WAVEHDR)); if ( mmRes != MMSYSERR_NOERROR ) { OutputErrorText( mmRes ); continue; } // WAVEデータを出力キューに入れる mmRes = waveOutWrite(hWaveOut, pHeader, sizeof(WAVEHDR)); if ( mmRes != MMSYSERR_NOERROR ) { OutputErrorText( mmRes ); continue; } // 次のバッファインデックス nAudioNextBuffer = (nAudioNextBuffer + 1) % NUMBER_OF_AUDIO_BUFFERS; } }
void gl_onDisplay() { gContext.WaitAndUpdateAll(); glClear(GL_COLOR_BUFFER_BIT); for(int i = 0; i < 2; i++){ if(gDrawOrder[i] != NULL){ gDrawOrder[i]->draw(); } } glutSwapBuffers(); }
// this function is called each frame void glutDisplay (void) { glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); XnMapOutputMode mode; g_DepthGenerator.GetMapOutputMode(mode); glOrtho(0, mode.nXRes, mode.nYRes, 0, -1.0, 1.0); glDisable(GL_TEXTURE_2D); g_Context.WaitAndUpdateAll(); g_pSessionManager->Update(&g_Context); PrintSessionState(g_SessionState); glutSwapBuffers(); }
void gl_onDisplay() { gContext.WaitAndUpdateAll(); glClear(GL_COLOR_BUFFER_BIT); gDepthDrawer->draw(); gSkeletonDrawer->draw(); gImageDrawer->draw(); if(capture->isCapturing()){ capture->captureSkeleton(gUserGenerator.GetSkeletonCap()); gl_labelCapturingUser(capture->getCapturingUser()); } glutSwapBuffers(); }
// this function is called each frame void glutDisplay (void) { glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); glDisable(GL_TEXTURE_2D); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } ros::Time tstamp=ros::Time::now(); // Process the data g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); DrawDepthMap(depthMD, sceneMD); std::vector<mapping_msgs::PolygonalMap> pmaps; body_msgs::Skeletons skels; getSkels(pmaps,skels); ROS_DEBUG("skels size %d \n",pmaps.size()); if(pmaps.size()){ skels.header.stamp=tstamp; skels.header.seq = depthMD.FrameID(); skels.header.frame_id="/openni_depth_optical_frame"; skel_pub.publish(skels); pmaps.front().header.stamp=tstamp; pmaps.front().header.seq = depthMD.FrameID(); pmaps.front().header.frame_id="/openni_depth_optical_frame"; pmap_pub.publish(pmaps[0]); } glutSwapBuffers(); }
// this function is called each frame void glutDisplay (void) { glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); #ifdef USE_GLUT glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); #else glOrthof(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); #endif glDisable(GL_TEXTURE_2D); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } // Process the data //DRAW g_DepthGenerator.GetMetaData(depthMD); g_SceneAnalyzer.GetMetaData(sceneMD); DrawDepthMap(depthMD, sceneMD); if (g_bPrintFrameID) { DrawFrameID(depthMD.FrameID()); } #ifdef USE_GLUT glutSwapBuffers(); #endif }
int main(int argc, char **argv) { ros::init(argc, argv, "scene_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"); } XnCallbackHandle hUserCallbacks; g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); 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; }
bool DataCapture::captureOne() { XnStatus rc = context_.WaitAndUpdateAll(); // want this to be WaitOneUpdateAll(RGB image) if( rc != XN_STATUS_OK ) { std::cout << "WaitAndUpdateAll: " << xnGetStatusString(rc) << std::endl; return false; } // grab image imageGen_.GetMetaData(imageMd_); const XnRGB24Pixel* rgbData = imageMd_.RGB24Data(); for( unsigned int i = 0; i < 640 * 480; ++i ) { pRgbData_[3*i] = rgbData->nRed; pRgbData_[3*i + 1] = rgbData->nGreen; pRgbData_[3*i + 2] = rgbData->nBlue; ++rgbData; } // grab depth image depthGen_.GetMetaData(depthMd_); const uint16_t* pDepthDataU16 = depthMd_.Data(); for( int i = 0; i < 640 * 480; ++i) { uint16_t d = pDepthDataU16[i]; if( d != 0 ) { pDepthData_[i] = (d * 255)/2048; } else { pDepthData_[i] = 0; // should be NAN } } return true; }
int main(int argc, char **argv) { ros::init(argc, argv, "kinnect"); ros::NodeHandle nh; string configFilename = ros::package::getPath("kinect") + "/config/openni.xml"; XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); CHECK_RC(nRetVal, "InitFromXml"); // init kinect user IDs to 0 to indicate they're invalid. for (int i = 0; i < NUSERS; i++) { kinectUsers[i].id = 0; } 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); ros::Publisher user_pub = nh.advertise<kinect::User>("kinect_users", 1000); while (ros::ok()) { g_Context.WaitAndUpdateAll(); publishTransforms(frame_id, user_pub); r.sleep(); } g_Context.Shutdown(); return 0; }
int main(int argc, char **argv){ ros::init(argc, argv, "pplTracker"); ros::NodeHandle nh; std::string configFilename = ros::package::getPath("people_tracker_denbyk") + "/init/openni_tracker.xml"; genericUserCalibrationFileName = ros::package::getPath("people_tracker_denbyk") + "/init/GenericUserCalibration.bin"; ros::Rate loop_rate(1); //valore di ritorno Xn XnStatus nRetVal; //while (ros::ok()) while (nh.ok()) { //inizializzo contesto openni //ROS_INFO(configFilename.c_str(),xnGetStatusString(nRetVal)); nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); //TODO: remove nRetVal = XN_STATUS_OK; //riprovo tra un po' if(nRetVal != XN_STATUS_OK) { ROS_INFO("InitFromXml failed: %s Retrying in 3 seconds...", xnGetStatusString(nRetVal)); ros::Duration(3).sleep(); } else { break; } } //std::string frame_id; nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); if(nRetVal != XN_STATUS_OK) { ROS_ERROR("Find depth generator failed: %s", xnGetStatusString(nRetVal)); } //cerca nodo ti tipo user generator e lo salva in g_UserGenerator nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { //crea lo userGenerator del g_context. SE non riesce probabilmente manca NITE nRetVal = g_UserGenerator.Create(g_Context); if (nRetVal != XN_STATUS_OK) { ROS_ERROR("NITE is likely missing: Please install NITE >= 1.5.2.21. Check the readme for download information. Error Info: User generator failed: %s", xnGetStatusString(nRetVal)); return nRetVal; } } //veriica che lo userGenerator creato supporti SKELETON if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { ROS_INFO("Supplied user generator doesn't support skeleton"); return EXIT_FAILURE; } //imposto la modalità dello skeleton, quali giunzioni rilevare e quali no. //in questo caso upper è il torso/la testa g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_UPPER); //setto varie callbacks per entrata, uscita e rientro user nello UserGenerator XnCallbackHandle hUserCallbacks; g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); g_UserGenerator.RegisterToUserExit(User_OutOfScene, NULL, hUserCallbacks); g_UserGenerator.RegisterToUserReEnter(User_BackIntoScene, NULL, hUserCallbacks); //attivo la generazione dei vari generators nRetVal = g_Context.StartGeneratingAll(); if(nRetVal != XN_STATUS_OK) { ROS_ERROR("StartGenerating failed: %s", xnGetStatusString(nRetVal)); } //recupera un parametro passato al nodo dal server dei parametri ROS. //usando la chiave camera_frame_id e lo memorizza nella variabile frame_id std::string frame_id("camera_depth_frame"); nh.getParam("camera_frame_id", frame_id); std::cout << "init ok\n"; //ciclo principale. while(nh.ok()) { //aggiorna il contesto e aspetta g_Context.WaitAndUpdateAll(); //pubblica le trasformazioni su frame_id publishTransforms(frame_id); //dormi per un tot. ros::Rate(30).sleep(); } //rilascia risorse e esci. g_Context.Shutdown(); return EXIT_SUCCESS; }
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 }
int main(int argc, char **argv) { ros::init(argc, argv, "openni_tracker"); ros::NodeHandle nh, nh_private("~"); ROS_INFO_STREAM("Initialising OpenNI tracker ..."); default_user = 0; available_tracked_users_pub = nh_private.advertise<std_msgs::UInt16MultiArray>("available_users", 10, true); default_user_pub = nh_private.advertise<std_msgs::UInt16>("tracked_user", 10, true); user_chooser_sub = nh_private.subscribe("user_chooser", 10, userChooserCallback); string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml"; ROS_INFO_STREAM("Setting up configuration from XML file '" << configFilename << "'"); XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); CHECK_RC(nRetVal, "InitFromXml"); ROS_INFO_STREAM("Looking for existing depth generators ..."); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); ROS_INFO_STREAM("Looking for existing user generators ..."); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); ROS_INFO_STREAM("No existing user generators found. Created new one."); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); } if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { ROS_INFO_STREAM("Supplied user generator doesn't support skeleton"); return 1; } ROS_INFO_STREAM("Registering user callbacks ..."); XnCallbackHandle hUserCallbacks; g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); ROS_INFO_STREAM("Registering calibration callbacks ..."); XnCallbackHandle hCalibrationCallbacks; g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); ROS_INFO_STREAM("Checking pose detection capability ..."); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { ROS_INFO_STREAM("Pose required, but not supported"); return 1; } ROS_INFO_STREAM("Registering pose callbacks ..."); XnCallbackHandle hPoseCallbacks; g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); ROS_INFO_STREAM("Getting calibration pose ..."); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } ROS_INFO_STREAM("Setting skeleton profile ..."); g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); //g_Context.Release(); ROS_INFO_STREAM("Starting to generate everything ..."); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); ROS_INFO_STREAM("Stopping to generate everything ..."); nRetVal = g_Context.StopGeneratingAll(); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); ROS_INFO_STREAM("Starting to generate everything ..."); nRetVal = g_Context.StartGeneratingAll(); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); ROS_INFO_STREAM("Setting up ROS node ..."); ros::Rate r(30); ros::NodeHandle pnh("~"); string frame_id("openni_depth_frame"); pnh.getParam("camera_frame_id", frame_id); nRetVal = g_Context.GetGlobalErrorState(); ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal)); ROS_INFO_STREAM("And go!"); while (ros::ok()) { ros::spinOnce(); nRetVal = g_Context.WaitAndUpdateAll(); CHECK_RC(nRetVal, "WaitAndUpdateAll"); publishTransforms(frame_id); r.sleep(); } g_Context.StopGeneratingAll(); g_Context.Release(); g_Context.Shutdown(); return 0; }
// this function is called each frame void glutDisplay (void) { glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Setup the OpenGL viewpoint glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); // Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator); // g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; xn::ImageMetaData imageMD; g_DepthGenerator.GetMetaData(depthMD); g_ImageGenerator.GetMetaData(imageMD); #ifdef USE_GLUT glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); #else glOrthof(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0); #endif glDisable(GL_TEXTURE_2D); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } // Process the data //DRAW // Check if Registration is done for Depth and RGB Images - Brandyn, Sravanthi g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator); // g_DepthGenerator.GetAlternativeViewPointCap().ResetViewPoint(); g_DepthGenerator.GetMetaData(depthMD); g_ImageGenerator.GetMetaData(imageMD); g_UserGenerator.GetUserPixels(0, sceneMD); DrawDepthMap(depthMD, imageMD, sceneMD, g_nPlayer); if (g_nPlayer != 0) { XnPoint3D com; g_UserGenerator.GetCoM(g_nPlayer, com); if (com.Z == 0) { g_nPlayer = 0; FindPlayer(); } } #ifdef USE_GLUT glutSwapBuffers(); #endif }
void glut_display() { xn::DepthMetaData pDepthMapMD; xn::ImageMetaData pImageMapMD; #ifdef DEBUGOUT ofstream datei; #endif glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); gluPerspective(45, WINDOW_SIZE_X/WINDOW_SIZE_Y, 1000, 5000); // glOrtho(0, WINDOW_SIZE_X, WINDOW_SIZE_Y, 0, -128, 128); glMatrixMode(GL_TEXTURE); glLoadIdentity(); // glTranslatef(-12.8/640.0, 9.0/480.0, 0); // glTranslatef(-12.8/630.0, 9.0/480.0,0); glScalef(scalex, scaley, 1.0); glTranslatef(transx/630, transy/480, 0.0); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); rot_angle+=0.7; // Warten auf neue Daten vom Tiefengenerator nRetVal = context.WaitAndUpdateAll(); checkError("Fehler beim Aktualisieren der Daten", nRetVal); // Aktuelle Depth Metadaten auslesen depth.GetMetaData(pDepthMapMD); // Aktuelle Depthmap auslesen const XnDepthPixel* pDepthMap = depth.GetDepthMap(); if(maxdepth==-1) maxdepth = getMaxDepth(pDepthMap); // Aktuelle Image Metadaten auslesen image.GetMetaData(pImageMapMD); //Aktuelles Bild auslesen const XnRGB24Pixel* pImageMap = image.GetRGB24ImageMap(); glColor3f(1, 1, 1); // XnDepthPixel maxdepth = depth.GetDeviceMaxDepth(); const unsigned int xres = pDepthMapMD.XRes(); const unsigned int yres = pDepthMapMD.YRes(); #ifdef DEBUGOUT datei.open("daniel.txt", ios::out); #endif for(unsigned int y=0; y<yres-1; y++) { for(unsigned int x=0; x<xres; x++) { aDepthMap[x+y*xres] = static_cast<GLubyte>(static_cast<float>(pDepthMap[x+y*xres])/static_cast<float>(maxdepth)*255); } } /* glEnable(GL_TEXTURE_2D); glPushMatrix(); glLoadIdentity(); glTranslatef(-800, 0, -2000); glBindTexture(GL_TEXTURE_2D, texture_rgb); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImageMap); glBegin(GL_QUADS); glTexCoord2f(0,1); glVertex3f(0,0,0); glTexCoord2f(1,1); glVertex3f(640,0,0); glTexCoord2f(1,0); glVertex3f(640,480,0); glTexCoord2f(0,0); glVertex3f(0,480,0); glEnd(); glPopMatrix(); glPushMatrix(); glLoadIdentity(); glTranslatef(-800, 600, -2000); glBindTexture(GL_TEXTURE_2D, texture_depth); glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE8, 640, 480, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, aDepthMap); glBegin(GL_QUADS); glTexCoord2f(0,1); glVertex3f(0,0,0); glTexCoord2f(1,1); glVertex3f(640,0,0); glTexCoord2f(1,0); glVertex3f(640,480,0); glTexCoord2f(0,0); glVertex3f(0,480,0); glEnd(); glPopMatrix();*/ glPushMatrix(); glLoadIdentity(); glTranslatef(-100, -100, -2000); glRotatef(cx,0,1,0); glRotatef(cy,1,0,0); glTranslatef(-320, -240, 1000); glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D, texture_rgb); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImageMap); glBegin(GL_POINTS); for(unsigned int y=0; y<yres-1; y++) { for(unsigned int x=0; x<630; x++) { if(pDepthMap[x+y*xres]!=0) { glTexCoord2f(static_cast<float>(x)/static_cast<float>(630), static_cast<float>(y)/static_cast<float>(480)); glVertex3f(x, (yres-y), -pDepthMap[x+y*xres]/2.00); #ifdef DEBUGOUT datei << t_gamma[pDepthMap[x+y*xres]] << endl; #endif } } } glEnd(); glPopMatrix(); glDisable(GL_TEXTURE_2D); glutSwapBuffers(); #ifdef DEBUGOUT datei.close(); exit(-1); #endif }
int main() { const unsigned int nBackgroundTrain = 30; const unsigned short touchDepthMin = 10; const unsigned short touchDepthMax = 20; const unsigned int touchMinArea = 50; const bool localClientMode = true; // connect to a local client const double debugFrameMaxDepth = 4000; // maximal distance (in millimeters) for 8 bit debug depth frame quantization const char* windowName = "Debug"; const char* colorWindowName = "image"; const Scalar debugColor0(0, 0, 128); const Scalar debugColor1(255, 0, 0); const Scalar debugColor2(255, 255, 255); const Scalar debugColor3(0, 255, 255); const Scalar debugColor4(255, 0, 255); int xMin = 50; int xMax = 550; int yMin = 50; int yMax = 300; Mat1s depth(480, 640); // 16 bit depth (in millimeters) Mat1b depth8(480, 640); // 8 bit depth Mat3b rgb(480, 640); // 8 bit depth Mat3b debug(480, 640); // debug visualization Mat1s foreground(640, 480); Mat1b foreground8(640, 480); Mat1b touch(640, 480); // touch mask Mat1s background(480, 640); vector<Mat1s> buffer(nBackgroundTrain); IplImage * image = cvCreateImage(cvSize(640, 480), 8, 3); IplImage * convertedImage = cvCreateImage(cvSize(640, 480), 8, 3); initOpenNI("niConfig.xml"); // TUIO server object TuioServer* tuio; if (localClientMode) { tuio = new TuioServer(); } else { tuio = new TuioServer("192.168.0.2", 3333, false); } TuioTime time; namedWindow(colorWindowName); createTrackbar("xMin", colorWindowName, &xMin, 640); createTrackbar("xMax", colorWindowName, &xMax, 640); createTrackbar("yMin", colorWindowName, &yMin, 480); createTrackbar("yMax", colorWindowName, &yMax, 480); // create some sliders namedWindow(windowName); createTrackbar("xMin", windowName, &xMin, 640); createTrackbar("xMax", windowName, &xMax, 640); createTrackbar("yMin", windowName, &yMin, 480); createTrackbar("yMax", windowName, &yMax, 480); Keyboard * piano = new Keyboard(); (*piano).initKeyMap(); system("qjackctl &"); sleep(4); JackByTheNotes * notesJack = new JackByTheNotes(); notesJack->connect(); sleep(2); system("sudo jack_connect Piano:Rubinstein system:playback_1 &"); map<double, timeval> keys; // create background model (average depth) for (unsigned int i = 0; i < nBackgroundTrain; i++) { xnContext.WaitAndUpdateAll(); depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); buffer[i] = depth; } average(buffer, background); while (waitKey(1) != 27) { // read available data xnContext.WaitAndUpdateAll(); // update 16 bit depth matrix depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); //xnImgeGenertor.GetGrayscale8ImageMap() XnRGB24Pixel* xnRgb = const_cast<XnRGB24Pixel*>(xnImgeGenertor.GetRGB24ImageMap()); // IplImage * image = cvCreateImage(cvSize(640, 480), 8, 3); // IplImage * convertedImage = cvCreateImage(cvSize(640, 480), 8, 3); cvSetData (image, xnRgb, 640 * 3); cvConvertImage(image, convertedImage, CV_CVTIMG_SWAP_RB); bool color = true; rgb = convertedImage; // cvtColor(rgb,rgb,CV_RGB2BGR); // update rgb image // rgb.data = (uchar*) xnImgeGenertor.GetRGB24ImageMap(); // segmentation fault here // cvCvtColor(rgb, rgb, CV_BGR2RGB); // extract foreground by simple subtraction of very basic background model foreground = background - depth; // find touch mask by thresholding (points that are close to background = touch points) touch = (foreground > touchDepthMin) & (foreground < touchDepthMax); // extract ROI Rect roi(xMin, yMin, xMax - xMin, yMax - yMin); Mat touchRoi = touch(roi); // find touch points vector<vector<Point2i> > contours; vector<Point2f> touchPoints; findContours(touchRoi, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, Point2i(xMin, yMin)); for (unsigned int i = 0; i < contours.size(); i++) { Mat contourMat(contours[i]); // find touch points by area thresholding if (contourArea(contourMat) > touchMinArea) { Scalar center = mean(contourMat); Point2i touchPoint(center[0], center[1]); touchPoints.push_back(touchPoint); } } // send TUIO cursors time = TuioTime::getSessionTime(); tuio->initFrame(time); for (unsigned int i = 0; i < touchPoints.size(); i++) { // touch points float cursorX = (touchPoints[i].x - xMin) / (xMax - xMin); float cursorY = 1 - (touchPoints[i].y - yMin) / (yMax - yMin); TuioCursor* cursor = tuio->getClosestTuioCursor(cursorX, cursorY); // TODO improve tracking (don't move cursors away, that might be closer to another touch point) if (cursor == NULL || cursor->getTuioTime() == time) { tuio->addTuioCursor(cursorX, cursorY); } else { tuio->updateTuioCursor(cursor, cursorX, cursorY); } } tuio->stopUntouchedMovingCursors(); tuio->removeUntouchedStoppedCursors(); tuio->commitFrame(); // draw debug frame depth.convertTo(depth8, CV_8U, 255 / debugFrameMaxDepth); // render depth to debug frame cvtColor(depth8, debug, CV_GRAY2BGR); debug.setTo(debugColor0, touch); // touch mask rectangle(debug, roi, debugColor1, 2); // surface boundaries if (color) rectangle(rgb, roi, debugColor1, 2); // surface boundaries // draw 10 white keys within the roi int stride = (xMax - xMin) / 10; for (int keys = 1; keys < 10; keys++) { Point lower(xMin + keys * stride, yMax); if (keys == 3 || keys == 7) { Point upper(xMin + keys * stride, yMin); line(debug, upper, lower, debugColor3, 2, 0); if (color) line(rgb, upper, lower, debugColor3, 2, 0); continue; } else { Point upper(xMin + keys * stride, (yMin + yMax) / 2); line(debug, upper, lower, debugColor3, 2, 0); if (color) line(rgb, upper, lower, debugColor3, 2, 0); } Point blkUpper(xMin + keys * stride - stride / 3, yMin); Point blkLower(xMin + keys * stride + stride / 3, (yMin + yMax) / 2); rectangle(debug, blkUpper, blkLower, debugColor4, 2); if (color) rectangle(rgb, blkUpper, blkLower, debugColor4, 2); } for (unsigned int i = 0; i < touchPoints.size(); i++) { // touch points circle(debug, touchPoints[i], 5, debugColor2, CV_FILLED); if (color) circle(rgb, touchPoints[i], 5, debugColor2, CV_FILLED); double frequency = piano->keyFrequency(touchPoints[i].y - 50, touchPoints[i].x - 50); cout << frequency << " " << touchPoints[i].y - 50 << " " << touchPoints[i].x - 50 << endl; if (keys.find(frequency) == keys.end()) { Note * note = new Note(frequency, 2, 4000); notesJack->playNote(*note); timeval now; gettimeofday(&now, NULL); keys[frequency] = now; } else { timeval now; gettimeofday(&now, NULL); if ((now.tv_sec - keys[frequency].tv_sec) * 1000 + (now.tv_usec - keys[frequency].tv_usec) / 1000 > 1000) { Note * note = new Note(frequency, 2, 4000); notesJack->playNote(*note); keys[frequency] = now; } } } // render debug frame (with sliders) // IplImage grayScale = debug; // cvFlip(&grayScale, NULL, 1); // Mat gray(&grayScale); // imshow(windowName, gray); // // IplImage colorful = rgb; // cvFlip(&colorful, NULL, 1); // Mat real(&colorful); // imshow("image", real); imshow(windowName, debug); imshow("image", rgb); // cvShowImage("image", image); } 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; }
bool OpenNIVideo::update(osg::NodeVisitor* nv) { //this is the main function of your video plugin //you can either retrieve images from your video stream/camera/file //or communicate with a thread to synchronize and get the data out //the most important is to synchronize your data //and copy the result to the VideoImageSteam used in this plugin // //0. you can collect some stats, for that you can use a timer osg::Timer t; { //1. mutex lock access to the image video stream OpenThreads::ScopedLock<OpenThreads::Mutex> _lock(this->getMutex()); osg::notify(osg::DEBUG_INFO)<<"osgART::OpenNIVideo::update() get new image.."<<std::endl; XnStatus nRetVal = XN_STATUS_OK; nRetVal=context.WaitAndUpdateAll(); CHECK_RC(nRetVal, "Update Data"); xnFPSMarkFrame(&xnFPS); depth_generator.GetMetaData(depthMD); const XnDepthPixel* pDepthMap = depthMD.Data(); //depth pixel floating point depth map. image_generator.GetMetaData(imageMD); const XnUInt8* pImageMap = imageMD.Data(); // Hybrid mode isn't supported in this sample if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes()) { std::cerr<<"The device depth and image resolution must be equal!"<<std::endl; exit(1); } // RGB is the only image format supported. if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { std::cerr<<"The device image format must be RGB24"<<std::endl; exit(1); } const XnDepthPixel* pDepth=pDepthMap; const XnUInt8* pImage=pImageMap; XnDepthPixel zMax = depthMD.ZRes(); //convert float buffer to unsigned short for ( unsigned int i=0; i<(depthMD.XRes() * depthMD.YRes()); ++i ) { *(_depthBufferByte + i) = 255 * (float(*(pDepth + i)) / float(zMax)); } memcpy(_videoStreamList[0]->data(),pImage, _videoStreamList[0]->getImageSizeInBytes()); memcpy(_videoStreamList[1]->data(),_depthBufferByte, _videoStreamList[1]->getImageSizeInBytes()); //3. don't forget to call this to notify the rest of the application //that you have a new video image _videoStreamList[0]->dirty(); _videoStreamList[1]->dirty(); } //4. hopefully report some interesting data if (nv) { const osg::FrameStamp *framestamp = nv->getFrameStamp(); if (framestamp && _stats.valid()) { _stats->setAttribute(framestamp->getFrameNumber(), "Capture time taken", t.time_m()); } } // Increase modified count every X ms to ensure tracker updates if (updateTimer.time_m() > 50) { _videoStreamList[0]->dirty(); _videoStreamList[1]->dirty(); updateTimer.setStartTick(); } return true; }
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, "tracker_2"); ros::NodeHandle nh; string configFilename = ros::package::getPath("openni_tracker_multi") + "/openni_tracker.xml"; XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str()); CHECK_RC(nRetVal, "InitFromXml"); // XnStatus nRetVal = XN_STATUS_OK; //xnLogInitFromXmlFile(csXmlFile); nRetVal = g_Context.Init(); XN_IS_STATUS_OK(nRetVal); // SELECTION OF THE DEVICE xn::EnumerationErrors errors; xn::Device g_Device; // find devices xn::NodeInfoList list; xn::NodeInfoList list_depth; nRetVal = g_Context.EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list, &errors); XN_IS_STATUS_OK(nRetVal); printf("The following devices were found:\n"); int i = 1; for (xn::NodeInfoList::Iterator it = list.Begin(); it != list.End(); ++it, ++i) { xn::NodeInfo deviceNodeInfo = *it; xn::Device deviceNode; deviceNodeInfo.GetInstance(deviceNode); XnBool bExists = deviceNode.IsValid(); if (!bExists) { g_Context.CreateProductionTree(deviceNodeInfo, deviceNode); // this might fail. } if (deviceNode.IsValid() && deviceNode.IsCapabilitySupported(XN_CAPABILITY_DEVICE_IDENTIFICATION)) { const XnUInt32 nStringBufferSize = 200; XnChar strDeviceName[nStringBufferSize]; XnChar strSerialNumber[nStringBufferSize]; XnUInt32 nLength = nStringBufferSize; deviceNode.GetIdentificationCap().GetDeviceName(strDeviceName, nLength); nLength = nStringBufferSize; deviceNode.GetIdentificationCap().GetSerialNumber(strSerialNumber, nLength); printf("[%d] %s (%s)\n", i, strDeviceName, strSerialNumber); } else { printf("[%d] %s\n", i, deviceNodeInfo.GetCreationInfo()); } // release the device if we created it if (!bExists && deviceNode.IsValid()) { deviceNode.Release(); } } printf("\n"); printf("Choose device to open (1): "); int chosen=0; if (argc != 2) { printf("Choose device to open (1): "); int nRetval = scanf("%d", &chosen); } else { chosen = atoi(argv[1]); } // create it xn::NodeInfoList::Iterator it = list.Begin(); for (i = 1; i < chosen; ++i) { it++; } xn::NodeInfo deviceNode = *it; nRetVal = g_Context.CreateProductionTree(deviceNode, g_Device); printf("Production tree of the device created.\n"); // SELECTION OF THE DEPTH GENERATOR nRetVal = g_Context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, NULL, list_depth, &errors); XN_IS_STATUS_OK(nRetVal); printf("The following devices were found:\n"); int i_depth = 1; for (xn::NodeInfoList::Iterator it_depth = list_depth.Begin(); it_depth != list_depth.End(); ++it_depth, ++i_depth) { xn::NodeInfo depthNodeInfo = *it_depth; xn::Device depthNode; depthNodeInfo.GetInstance(depthNode); XnBool bExists_depth = depthNode.IsValid(); if (!bExists_depth) { g_Context.CreateProductionTree(depthNodeInfo, depthNode); // this might fail. } if (depthNode.IsValid() && depthNode.IsCapabilitySupported(XN_CAPABILITY_DEVICE_IDENTIFICATION)) { const XnUInt32 nStringBufferSize = 200; XnChar strDeviceName[nStringBufferSize]; XnChar strSerialNumber[nStringBufferSize]; XnUInt32 nLength = nStringBufferSize; depthNode.GetIdentificationCap().GetDeviceName(strDeviceName, nLength); nLength = nStringBufferSize; depthNode.GetIdentificationCap().GetSerialNumber(strSerialNumber, nLength); printf("[%d] %s (%s)\n", i, strDeviceName, strSerialNumber); } else { printf("[%d] %s\n", i, depthNodeInfo.GetCreationInfo()); } // release the device if we created it if (!bExists_depth && depthNode.IsValid()) { depthNode.Release(); } } printf("\n"); printf("Choose device to open (1): "); int chosen_depth = 1; /*if (argc != 2) { printf("Choose device to open (1): "); int nRetval_depth = scanf("%d", &chosen_depth); } else { chosen_depth = atoi(argv[1]); }*/ // create it xn::NodeInfoList::Iterator it_depth = list_depth.Begin(); for (i = 1; i < chosen_depth; ++i) { it_depth++; } xn::NodeInfo depthNode = *it_depth; nRetVal = g_Context.CreateProductionTree(depthNode, g_DepthGenerator); printf("Production tree of the DepthGenerator created.\n"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); printf("Production tree of the depth generator created.\n"); XN_IS_STATUS_OK(nRetVal); printf("XN_IS_STATUS_OK(nRetVal).\n"); CHECK_RC(nRetVal, "Find depth generator"); printf("CHECK_RC(nRetVal, Find depth generator);\n"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); printf("User generator found.\n"); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); printf("User generator created.\n"); CHECK_RC(nRetVal, "Find user generator"); } if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton.\n"); 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("/kinect2_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; }
void Context_WaitAndUpdateAll_wrapped(xn::Context& self) { check( self.WaitAndUpdateAll() ); }
void updateKinnect() { xnContext.WaitAndUpdateAll(); }
int main() { const unsigned int nBackgroundTrain = 30; const unsigned short touchDepthMin = 10; const unsigned short touchDepthMax = 20; const unsigned int touchMinArea = 50; const bool localClientMode = true; // connect to a local client const double debugFrameMaxDepth = 4000; // maximal distance (in millimeters) for 8 bit debug depth frame quantization const char* windowName = "Debug"; const Scalar debugColor0(0,0,128); const Scalar debugColor1(255,0,0); const Scalar debugColor2(255,255,255); int xMin = 110; int xMax = 560; int yMin = 120; int yMax = 320; Mat1s depth(480, 640); // 16 bit depth (in millimeters) Mat1b depth8(480, 640); // 8 bit depth Mat3b rgb(480, 640); // 8 bit depth Mat3b debug(480, 640); // debug visualization Mat1s foreground(640, 480); Mat1b foreground8(640, 480); Mat1b touch(640, 480); // touch mask Mat1s background(480, 640); vector<Mat1s> buffer(nBackgroundTrain); CHECK_RC(initOpenNI("niConfig.xml"), "initOpenNI"); // TUIO server object TuioServer* tuio; if (localClientMode) { tuio = new TuioServer(); } else { tuio = new TuioServer("192.168.0.1",3333,false); } TuioTime time; // create some sliders namedWindow(windowName); createTrackbar("xMin", windowName, &xMin, 640); createTrackbar("xMax", windowName, &xMax, 640); createTrackbar("yMin", windowName, &yMin, 480); createTrackbar("yMax", windowName, &yMax, 480); // create background model (average depth) for (unsigned int i=0; i<nBackgroundTrain; i++) { CHECK_RC(xnContext.WaitAndUpdateAll(), "xnContext.WaitAndUpdateAll()"); depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); buffer[i] = depth; } average(buffer, background); while ( waitKey(1) != 27 ) { // read available data xnContext.WaitAndUpdateAll(); // update 16 bit depth matrix depth.data = (uchar*) xnDepthGenerator.GetDepthMap(); //xnImgeGenertor.GetGrayscale8ImageMap() // update rgb image //rgb.data = (uchar*) xnImgeGenertor.GetRGB24ImageMap(); // segmentation fault here //cvtColor(rgb, rgb, CV_RGB2BGR); // extract foreground by simple subtraction of very basic background model foreground = background - depth; // find touch mask by thresholding (points that are close to background = touch points) touch = (foreground > touchDepthMin) & (foreground < touchDepthMax); // extract ROI Rect roi(xMin, yMin, xMax - xMin, yMax - yMin); Mat touchRoi = touch(roi); // find touch points vector< vector<Point2i> > contours; vector<Point2f> touchPoints; findContours(touchRoi, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, Point2i(xMin, yMin)); for (unsigned int i=0; i<contours.size(); i++) { Mat contourMat(contours[i]); // find touch points by area thresholding if ( contourArea(contourMat) > touchMinArea ) { Scalar center = mean(contourMat); Point2i touchPoint(center[0], center[1]); touchPoints.push_back(touchPoint); } } // send TUIO cursors time = TuioTime::getSessionTime(); tuio->initFrame(time); for (unsigned int i=0; i<touchPoints.size(); i++) { // touch points float cursorX = (touchPoints[i].x - xMin) / (xMax - xMin); float cursorY = 1 - (touchPoints[i].y - yMin)/(yMax - yMin); TuioCursor* cursor = tuio->getClosestTuioCursor(cursorX,cursorY); // TODO improve tracking (don't move cursors away, that might be closer to another touch point) if (cursor == NULL || cursor->getTuioTime() == time) { tuio->addTuioCursor(cursorX,cursorY); } else { tuio->updateTuioCursor(cursor, cursorX, cursorY); } } tuio->stopUntouchedMovingCursors(); tuio->removeUntouchedStoppedCursors(); tuio->commitFrame(); // draw debug frame depth.convertTo(depth8, CV_8U, 255 / debugFrameMaxDepth); // render depth to debug frame cvtColor(depth8, debug, CV_GRAY2BGR); debug.setTo(debugColor0, touch); // touch mask rectangle(debug, roi, debugColor1, 2); // surface boundaries for (unsigned int i=0; i<touchPoints.size(); i++) { // touch points circle(debug, touchPoints[i], 5, debugColor2, CV_FILLED); } // render debug frame (with sliders) imshow(windowName, debug); //imshow("image", rgb); } return 0; }
// this function is called each frame void glutDisplay (void) { if(gScene == NULL) return; gScene->simulate(1.0f/30.0f); glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // 射影マトリックス glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(100.0f, (float)glutGet(GLUT_WINDOW_WIDTH)/(float)glutGet(GLUT_WINDOW_HEIGHT), 1.0f, 10000.0f); // 視点の位置 glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(gEye.x, gEye.y, gEye.z, gEye.x + gDir.x, gEye.y + gDir.y, gEye.z + gDir.z, 0.0f, 1.0f, 0.0f); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); if (!g_bPause) { // Read next available data g_Context.WaitAndUpdateAll(); } for(int i=0;i<2;i++){ for(int j=0;j<15;j++){ oldjpoint[i][j]=jpoint[i][j]; } } // Process the data g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); CalculateJoint(); head.x=(jpoint[0][0].X); head.y=(jpoint[0][0].Y); head.z=(jpoint[0][0].Z); neck.x=(jpoint[0][1].X); neck.y=(jpoint[0][1].Y); neck.z=(jpoint[0][1].Z); rshoulder.x=(jpoint[0][2].X); rshoulder.y=(jpoint[0][2].Y); rshoulder.z=(jpoint[0][2].Z); relbow.x=(jpoint[0][3].X*2+oldjpoint[0][3].X)/3; relbow.y=(jpoint[0][3].Y*2+oldjpoint[0][3].Y)/3; relbow.z=(jpoint[0][3].Z*2+oldjpoint[0][3].Z)/3; rhand.x=(jpoint[0][4].X*2+oldjpoint[0][4].X)/3; rhand.y=(jpoint[0][4].Y*2+oldjpoint[0][4].Y)/3; rhand.z=(jpoint[0][4].Z*2+oldjpoint[0][4].Z)/3; lshoulder.x=(jpoint[0][5].X*2+oldjpoint[0][5].X)/3; lshoulder.y=(jpoint[0][5].Y*2+oldjpoint[0][5].Y)/3; lshoulder.z=(jpoint[0][5].Z*2+oldjpoint[0][5].Z)/3; lelbow.x=(jpoint[0][6].X*2+oldjpoint[0][6].X)/3; lelbow.y=(jpoint[0][6].Y*2+oldjpoint[0][6].Y)/3; lelbow.z=(jpoint[0][6].Z*2+oldjpoint[0][6].Z)/3; lhand.x=(jpoint[0][7].X*2+oldjpoint[0][7].X)/3; lhand.y=(jpoint[0][7].Y*2+oldjpoint[0][7].Y)/3; lhand.z=(jpoint[0][7].Z*2+oldjpoint[0][7].Z)/3; torso.x=(jpoint[0][8].X*2+oldjpoint[0][8].X)/3; torso.y=(jpoint[0][8].Y*2+oldjpoint[0][8].Y)/3; torso.z=(jpoint[0][8].Z*2+oldjpoint[0][8].Z)/3; rhip.x=(jpoint[0][9].X*2+oldjpoint[0][9].X)/3; rhip.y=(jpoint[0][9].Y*2+oldjpoint[0][9].Y)/3; rhip.z=(jpoint[0][9].Z*2+oldjpoint[0][9].Z)/3; rknee.x=(jpoint[0][10].X*2+oldjpoint[0][10].X)/3; rknee.y=(jpoint[0][10].Y*2+oldjpoint[0][10].Y)/3; rknee.z=(jpoint[0][10].Z*2+oldjpoint[0][10].Z)/3; rfoot.x=(jpoint[0][11].X*2+oldjpoint[0][11].X)/3; rfoot.y=(jpoint[0][11].Y*2+oldjpoint[0][11].Y)/3; rfoot.z=(jpoint[0][11].Z*2+oldjpoint[0][11].Z)/3; lhip.x=(jpoint[0][12].X*2+oldjpoint[0][12].X)/3; lhip.y=(jpoint[0][12].Y*2+oldjpoint[0][12].Y)/3; lhip.z=(jpoint[0][12].Z*2+oldjpoint[0][12].Z)/3; lknee.x=(jpoint[0][13].X*2+oldjpoint[0][13].X)/3; lknee.y=(jpoint[0][13].Y*2+oldjpoint[0][13].Y)/3; lknee.z=(jpoint[0][13].Z*2+oldjpoint[0][13].Z)/3; lfoot.x=(jpoint[0][14].X*2+oldjpoint[0][14].X)/3; lfoot.y=(jpoint[0][14].Y*2+oldjpoint[0][14].Y)/3; lfoot.z=(jpoint[0][14].Z*2+oldjpoint[0][14].Z)/3; printf("%f, %f, %f\n",rightreduction.x, rightreduction.y, rightreduction.z); printf("%f, %f, %f\n",leftreduction.x, leftreduction.y, leftreduction.z); if(jpoint[0][8].X!=0.0&&jpoint[0][8].Y!=0.0&&jpoint[0][8].Z!=0.0){ Head->setGlobalPosition(NxVec3(-head.x+positionx, -head.y+positiony, -head.z+positionz)); Head->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); Neck->setGlobalPosition(NxVec3(-neck.x+positionx, -neck.y+positiony, -neck.z+positionz)); Neck->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightshoulder->setGlobalPosition(NxVec3(-rshoulder.x+positionx, -rshoulder.y+positiony, -rshoulder.z+positionz)); rightshoulder->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightelbow->setGlobalPosition(NxVec3(-relbow.x+positionx, -relbow.y+positiony, -relbow.z+positionz)); rightelbow->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); righthand->setGlobalPosition(NxVec3(-rhand.x+positionx, -rhand.y+positiony, -rhand.z+positionz)); righthand->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftshoulder->setGlobalPosition(NxVec3(-lshoulder.x+positionx, -lshoulder.y+positiony, -lshoulder.z+positionz)); leftshoulder->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftelbow->setGlobalPosition(NxVec3(-lelbow.x+positionx, -lelbow.y+positiony, -lelbow.z+positionz)); leftelbow->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); lefthand->setGlobalPosition(NxVec3(-lhand.x+positionx, -lhand.y+positiony, -lhand.z+positionz)); lefthand->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); Torso->setGlobalPosition(NxVec3(-torso.x+positionx, -torso.y+positiony, -torso.z+positionz)); Torso->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); righthip->setGlobalPosition(NxVec3(-rhip.x+positionx, -rhip.y+positiony, -rhip.z+positionz)); righthip->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightknee->setGlobalPosition(NxVec3(-rknee.x+positionx, -rknee.y+positiony, -rknee.z+positionz)); rightknee->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); rightfoot->setGlobalPosition(NxVec3(-rfoot.x+positionx, -rfoot.y+positiony, -rfoot.z+positionz)); rightfoot->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); lefthip->setGlobalPosition(NxVec3(-lhip.x+positionx, -lhip.y+positiony, -lhip.z+positionz)); lefthip->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftknee->setGlobalPosition(NxVec3(-lknee.x+positionx, -lknee.y+positiony, -lknee.z+positionz)); leftknee->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); leftfoot->setGlobalPosition(NxVec3(-lfoot.x+positionx, -lfoot.y+positiony, -lfoot.z+positionz)); leftfoot->setLinearVelocity(NxVec3(0.0f, 0.0f, 0.0f)); } glPushMatrix(); glBegin(GL_POLYGON); glColor4f(0.2f,1.2f,0.1f,1.0f); // 地面の色 glVertex3f( 10000.0f,-1.0f, 10000.0f); glVertex3f( 10000.0f,-1.0f,-10000.0f); glVertex3f(-10000.0f,-1.0f,-10000.0f); glVertex3f(-10000.0f,-1.0f, 10000.0f); glEnd(); glPopMatrix(); //地平線の描画 glPushMatrix(); glColor4f(1.0f, 1.0f, 1.0f,1.0f); glLineWidth(1); glBegin(GL_LINES); for(int i=-10000; i< 10000; i+=500) { glVertex3f( i , -0.2f,-10000 ); glVertex3f( i , -0.2f, 10000 ); glVertex3f(-10000, -0.2f, i ); glVertex3f( 10000, -0.2f, i ); } glEnd(); glPopMatrix(); switch(gameflag){ case 0: // スタート画面 glViewport( 0, 0, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT) ); glMatrixMode( GL_PROJECTION ); glLoadIdentity(); glOrtho( 0.0f, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT), 0.0f, 0.0f, 1.0f ); glMatrixMode( GL_MODELVIEW ); glLoadIdentity(); glPushMatrix(); glColor3d(1.0, 1.0, 0.0); glRasterPos3d(GL_WIN_SIZE_X/2-80, GL_WIN_SIZE_Y/2, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,"Push 's' -> START"); glPopMatrix(); printf("Push 's' -> START\n"); break; case 1: // ゲーム終了時 glViewport( 0, 0, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT) ); glMatrixMode( GL_PROJECTION ); glLoadIdentity(); glOrtho( 0.0f, (float)glutGet(GLUT_WINDOW_WIDTH),(float)glutGet(GLUT_WINDOW_HEIGHT), 0.0f, 0.0f, 1.0f ); glMatrixMode( GL_MODELVIEW ); glLoadIdentity(); glPushMatrix(); glColor3d(0.0, 0.0, 1.0); glRasterPos3d(GL_WIN_SIZE_X/2-60, GL_WIN_SIZE_Y/2+20, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,"GAME OVER"); glPopMatrix(); glPushMatrix(); glColor3d(1.0, 1.0, 0.0); glRasterPos3d(GL_WIN_SIZE_X/2-65, GL_WIN_SIZE_Y/2, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,gametime); glPopMatrix(); glPushMatrix(); glColor3d(1.0, 0.0, 0.0); glRasterPos3d(GL_WIN_SIZE_X/2-90, GL_WIN_SIZE_Y/2-20, 0.0); drawBitmapString(GLUT_BITMAP_HELVETICA_18,"Push 'r' -> RESTART"); glPopMatrix(); printf("GAME OVER\ntime -> %f\n",(double)(t2 - t1) / CLOCKS_PER_SEC); break; case 2: // ゲーム中の画面 glEnable(GL_DEPTH_TEST); // アクターの描画 int nbActors = gScene->getNbActors(); NxActor** actors = gScene->getActors(); while(nbActors--) { NxActor* actor = *actors++; if(!actor->userData) continue; glPushMatrix(); glEnable(GL_LIGHTING); float glMat[16]; actor->getGlobalPose().getColumnMajor44(glMat); glMultMatrixf(glMat); myData* mydata = (myData*)actor->userData; int shape = mydata->shape; int color = mydata->color; switch(shape){ case 1: glColor4f(0.0f,0.0f,1.0f,1.0f); if(mydata->houkou==0){ link(pole[0], pole[4]); } else if(mydata->houkou==1){ tlink(3000.0,2000.0,6000.0,-3000.0,2000.0,6000.0); tlink(-3000.0,2000.0,4000.0,-3000.0,2000.0,6000.0); } else if(mydata->houkou==2){ ylink(3000.0,2000.0,4000.0,-3000.0,2000.0,4000.0); ylink(3000.0,2000.0,6000.0,-3000.0,2000.0,6000.0); } else if(mydata->houkou==3){ // パネル glColor4f(0.0f,1.0f,1.0f,1.0f); cuboid(mydata->width*2,mydata->length*2, mydata->height*2); } else if(mydata->houkou==4){ } break; case 2: if(mydata->ball == 1){ glColor4f(1.0f,0.0f,0.0f,1.0f); // ボールの色 glutSolidSphere(mydata->size,15,15); } else if(mydata->ball == 0){ glColor4f(1.0f,1.0f,0.0f,1.0f); glutSolidSphere(mydata->size,15,15); } break; case 4: glColor4f(1.0f,0.0f,0.0f,1.0f); cylinder(50, 100, 50); break; } glDisable(GL_LIGHTING); glPopMatrix(); } glDisable(GL_DEPTH_TEST); t2 = clock(); balltimer2 = clock(); btime=(double)(balltimer2 - balltimer1) / CLOCKS_PER_SEC; gtime=(double)(t2 - t1) / CLOCKS_PER_SEC; printf("%f\n",gtime); if(gtime<=10.0){ balldire=2.0; ballspeed=250.0; } else if(gtime<=20.0){ balldire=1.5; ballspeed=300.0; } else if(gtime<=30.0){ balldire=1.0; ballspeed=350.0; } else if(gtime<=40.0){ balldire=0.5; ballspeed=400.0; } break; } // 描画の終了 gScene->flushStream(); gScene->fetchResults(NX_RIGID_BODY_FINISHED, true); glutSwapBuffers(); }
int main(int argc, char **argv) { ros::init(argc, argv, "openni_hand_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"); // Create generators nRetVal = g_GestureGenerator.Create(g_Context); CHECK_RC(nRetVal, "Unable to create GestureGenerator."); nRetVal = g_HandsGenerator.Create(g_Context); CHECK_RC(nRetVal, "Unable to create HandsGenerator."); ROS_INFO("Create Generator Success"); /* 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(); r.sleep(); } g_Context.Shutdown(); return 0; }
void OniTrackApp::update() { context.WaitAndUpdateAll(); ((XnVSessionManager*)pSessionGenerator)->Update(&context); }