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
}
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;
}
