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