void runOnce (void)
{
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD_;
// Read next available data
// by Heresy, wait original depth generator
g_Context.WaitOneUpdateAll( g_DepthGenerator );
// Process the data
g_DepthGenerator.GetMetaData (depthMD_);
g_DepthGenerator.GetMetaData (depthMD);
depthMD_.MakeDataWritable();
xn::DepthMap& depthMap_ = depthMD_.WritableDepthMap();
xn::DepthMap& depthMap = depthMD.WritableDepthMap();
static float *buffer = 0;
if (buffer == 0)
{
std::cout << "(re)allocating depth buffer" << std::endl;
buffer = (float*) malloc (depthMap.XRes() * depthMap.YRes() * sizeof(float));
}
for (XnUInt y = 0; y < depthMap.YRes(); y++)
{
for (XnUInt x = 0; x < depthMap.XRes(); x++)
{
buffer [x + y * depthMap.XRes()] = depthMap (depthMap.XRes() - x - 1, y) * 0.001;
}
}
float P[12];
P[0] = 585.260; P[1] = 0.0; P[2] = 317.387; P[3] = 0.0;
P[4] = 0.0; P[5] = 585.028; P[6] = 239.264; P[7] = 0.0;
P[8] = 0.0; P[9] = 0.0; P[10] = 1.0; P[11] = 0.0;
double fx = P[0];
double fy = P[5];
double cx = P[2];
double cy = P[6];
double far_plane_ = 8;
double near_plane_ = 0.1;
double glTf[16];
for (unsigned int i = 0; i < 16; ++i)
glTf[i] = 0.0;
// calculate the projection matrix
// NOTE: this minus is there to flip the x-axis of the image.
glTf[0]= -2.0 * fx / depthMap.XRes();
glTf[5]= 2.0 * fy / depthMap.YRes();
glTf[8]= 2.0 * (0.5 - cx / depthMap.XRes());
glTf[9]= 2.0 * (cy / depthMap.YRes() - 0.5);
glTf[10]= - (far_plane_ + near_plane_) / (far_plane_ - near_plane_);
glTf[14]= -2.0 * far_plane_ * near_plane_ / (far_plane_ - near_plane_);
glTf[11]= -1;
filter->filter ((unsigned char*)buffer, glTf, depthMap.XRes(), depthMap.YRes());
GLfloat* masked_depth = filter->getMaskedDepth();
for (XnUInt y = 0; y < depthMap.YRes(); y++)
{
for (XnUInt x = 0; x < depthMap.XRes(); x++)
{
depthMap_(x, y) = XnDepthPixel (masked_depth[x + y * depthMap.XRes()] * 1000);
}
}
mockDepth.SetData(depthMD_);
publishTransforms (std::string("openni_depth_frame"));
}
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;
}
