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]);
if (nRetVal != XN_STATUS_OK)
{
printf("Can't open recording %s: %s\n", argv[1], xnGetStatusString(nRetVal));
return 1;
}
}
else
{
nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH);
CHECK_RC(nRetVal, "InitFromXml");
}
g_tunnel = new Tunnel();
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();
}
bool kinect_reader2::init() {
nRetVal = XN_STATUS_OK;
nRetVal = g_Context.Init();
if (nRetVal != XN_STATUS_OK) {
printf("Creating context failed: %s\n", xnGetStatusString(nRetVal));
return false;
}
nRetVal = g_DepthGenerator.Create(g_Context);
if(nRetVal != XN_STATUS_OK) {
printf("Creating depth generator failed: %s\n", xnGetStatusString(nRetVal));
return false;
}
g_DepthGenerator.GetMirrorCap().SetMirror(true);
nRetVal = g_UserGenerator.Create(g_Context);
if(nRetVal != XN_STATUS_OK) {
printf("Creating user generator failed: %s\n", xnGetStatusString(nRetVal));
return false;
}
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return false;
}
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("Pose required, but not supported\n");
return false;
}
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
g_Context.StartGeneratingAll();
return true;
}
information()
{
RC(context.Init(), "Context Intialized");
XnMapOutputMode mode;
mode.nXRes = XN_VGA_X_RES;
mode.nYRes = XN_VGA_Y_RES;
mode.nFPS = 30;
RC(image.Create(context), "Create image buffer");
RC(image.SetMapOutputMode(mode), "Set image mode");
RC(depth.Create(context), "Create depth buffer");
RC(depth.SetMapOutputMode(mode), "Set depth mode");
xn::Query q;
RC(q.AddSupportedCapability(XN_CAPABILITY_SKELETON), "Request skeleton");
try {
RC(context.FindExistingNode(XN_NODE_TYPE_USER, user), "User generator");
} catch (...) {
RC(user.Create(context), "Get skeleton!!!");
}
// RC(user.Create(context, &q), "Get skeleton!!!");
//
// xn::NodeInfoList il;
// RC(context.EnumerateProductionTrees(XN_NODE_TYPE_USER, &q, il, NULL),
// "Enumerate nodes");
//
// xn::NodeInfo i = *il.Begin();
// RC(context.CreateProductionTree(i), "Create skeleton node");
// RC(i.GetInstance(user), "Get skeleton");
user.RegisterUserCallbacks(User_NewUser, NULL, NULL, hUserCallbacks);
user.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, &user, hCalibrationCallbacks);
if (user.GetSkeletonCap().NeedPoseForCalibration())
{
if (!user.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
post("Pose required, but not supported\n");
}
else
{
user.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, &user, hPoseCallbacks);
user.GetSkeletonCap().GetCalibrationPose(g_strPose);
user.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
}
}
RC(context.StartGeneratingAll(), "Start generating data");
post("Kinect initialized!\n");
}
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();
}
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;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "tts_talkback_to_openni_tracker");
ros::NodeHandle nh;
// 2015.5.11 add
//voice = nh.getParam("~voice", voice_cmu_us_clb_arctic_clunits);
sound_play::SoundClient soundhandle;
//soundhandle = sound_play::SoundClient();
soundhandle.stopAll();
//soundhandle.say("Ready", voice);
soundhandle.say("Ready");
ROS_INFO("waiting for openni_tracker package to detect people ...");
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);
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;
}
}
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)
{
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();
}
int main(int argc, char **argv)
{
setup(argc, argv, &g_UserGenerator);
XnStatus nRetVal = XN_STATUS_OK;
std::string configFilename = ros::package::getPath("user_tracker") + "/config/SamplesConfig.xml";
xn::EnumerationErrors errors;
nRetVal = g_Context.InitFromXmlFile(configFilename.c_str(), g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
ROS_INFO("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
ROS_INFO("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
if (nRetVal != XN_STATUS_OK)
{
ROS_INFO("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))
{
ROS_INFO("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))
{
ROS_INFO("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_UserGenerator.GetSkeletonCap().RegisterToCalibrationInProgress(MyCalibrationInProgress, NULL, hCalibrationInProgress);
CHECK_RC(nRetVal, "Register to calibration in progress");
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseInProgress(MyPoseInProgress, NULL, hPoseInProgress);
CHECK_RC(nRetVal, "Register to pose 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))
{
ROS_INFO("Error initializing opengles\n");
CleanupExit();
}
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
while (!g_bQuit)
{
glutDisplay();
eglSwapBuffers(display, surface);
}
opengles_shutdown(display, surface, context);
CleanupExit();
#endif
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
TotalFrames = 0;
static struct rusage ru;
long double t;
double w;
struct timeval timStart;
struct timeval timEnd;
struct timeval Wall;
bool Sample = true;
XnFPSData xnFPS;
XnUserID aUsers[MAX_NUM_USERS];
XnUInt16 nUsers;
XnSkeletonJointTransformation torsoJoint;
if (argc > 1)
{
//parse the cmd line
if (strcasecmp(argv[1],"--help") == 0)
{
PrintHelpHeader(argv[0]);
return 1;
}
numOfUser = atoi(argv[1]);
if(numOfUser == 0)
{
PrintHelpHeader(argv[0]);
return 1;
}
else if(numOfUser > 2)
{
printf("Maximal Users allowed is 2\n");
return 1;
}
}
else
{
numOfUser = 4;
}
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");
#if (XN_PLATFORM != XN_PLATFORM_MACOSX)
//we want out benchmark application will be running only on one CPU core
cpu_set_t mask;
CPU_ZERO(&mask);
CPU_SET(1,&mask);
sched_setaffinity(0,sizeof(mask),&mask);
#endif
//initialize the FPS calculator
nRetVal = xnFPSInit(&xnFPS, 90);
CHECK_RC(nRetVal, "FPS Init");
//ensure the User generator exists
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");
}
//register to generators callbacks
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");
//ensure calibration
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);
}
//set skeleton profile (all joints)
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
//start to generate
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
printf("%c[%d;%d;%dmPrimeSense Skeleton Benchmark Application\n ", 0x1B, BRIGHT,BLUE,BG_BLACK);
printf("%c[%d;%d;%dmSet Maximal users to %d ", 0x1B, BRIGHT,BLUE,BG_BLACK,numOfUser);
printf("%c[%dm\n", 0x1B, 0);
printf("Starting to run\n");
if(g_bNeedPose)
{
printf("Assume calibration pose\n");
}
XnUInt32 epochTime = 0;
//each 30 frames (1 second) we start the CPU resources usages
while (!xnOSWasKeyboardHit())
{
if (Sample)
{ //get the beginning sample of CPU resources
getrusage(RUSAGE_SELF, &ru);
timStart=ru.ru_utime;
t=(double)timStart.tv_sec * 1000000.0 + (double)timStart.tv_usec \
+ (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec;
//get the wall clock time
gettimeofday(&Wall,NULL);
w=(double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec;
Sample = false;
}
g_Context.WaitOneUpdateAll(g_UserGenerator);
xnFPSMarkFrame(&xnFPS);
// print the torso information for the first user already tracking every 1 second to prevent CPU of printf
if(TotalFrames % 30 == 0)
{
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;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint);
printf("User %d Located At distance of %6.2f mm from the sensor\n",aUsers[i],torsoJoint.position.position.Z);
}
//get the finish sample of the CPU resources
getrusage(RUSAGE_SELF, &ru);
timEnd=ru.ru_utime;
t = (double)timEnd.tv_sec * 1000000.0 + (double)timEnd.tv_usec \
+ (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec - t;
//get the wall clock
gettimeofday(&Wall,NULL);
w = (double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec - w;
XnDouble fps=xnFPSCalc(&xnFPS);
//print stuff.
printf("%c[%d;%d;%dmCPU Utilization=%3.2f%%\t", 0x1B, BRIGHT,RED,BG_BLACK,(double)100*t/w);
printf("%c[%d;%d;%dmFPS=%3.2f ", 0x1B, BRIGHT,RED,BG_BLACK,(double)fps);
printf("%c[%dm\n", 0x1B, 0);
Sample= true;
}
TotalFrames++;
}
g_scriptNode.Release();
g_DepthGenerator.Release();
g_UserGenerator.Release();
g_Context.Release();
}
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;
}
void KinectDataPub::initOpenNI(const char* fn)
{
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
if (!fileExists(fn)){
printf("Could not find '%s'. Aborting.\n", fn);
exit(XN_STATUS_ERROR);
}
nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT){
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
exit(nRetVal);
}
else if (nRetVal != XN_STATUS_OK){
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
exit(nRetVal);
}
printf("Reading config from '%s'\n", fn);
nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthGen);
CHECK_RC(nRetVal, "Find depth generator");
nRetVal = context.FindExistingNode(XN_NODE_TYPE_USER, userGen);
if (nRetVal != XN_STATUS_OK){
nRetVal = userGen.Create(context);
CHECK_RC(nRetVal, "Find user generator");
}
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete,
hPoseDetected;
if (!userGen.IsCapabilitySupported(XN_CAPABILITY_SKELETON)){
printf("Supplied user generator doesn't support skeleton\n");
exit(EXIT_FAILURE);
}
nRetVal = userGen.RegisterUserCallbacks(newUserCB, lostUserCB, NULL,
hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
nRetVal = userGen.GetSkeletonCap().
RegisterToCalibrationStart(calibrationStartCB, NULL, hCalibrationStart);
CHECK_RC(nRetVal, "Register to calibration start");
nRetVal = userGen.GetSkeletonCap().
RegisterToCalibrationComplete(calibrationCompleteCB, NULL,
hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
if (userGen.GetSkeletonCap().NeedPoseForCalibration()){
needPose = TRUE;
if (!userGen.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)){
printf("Pose required, but not supported\n");
exit(EXIT_FAILURE);
}
nRetVal = userGen.GetPoseDetectionCap().
RegisterToPoseDetected(poseDetectedCB, NULL, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
userGen.GetSkeletonCap().GetCalibrationPose(strPose);
}
userGen.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
userGen.GetSkeletonCap().SetSmoothing(0.1f);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "openni_tracker");
ros::NodeHandle nh, nh_private("~");
ROS_INFO_STREAM("Initialising OpenNI tracker ...");
default_user = 0;
available_tracked_users_pub = nh_private.advertise<std_msgs::UInt16MultiArray>("available_users", 10, true);
default_user_pub = nh_private.advertise<std_msgs::UInt16>("tracked_user", 10, true);
user_chooser_sub = nh_private.subscribe("user_chooser", 10, userChooserCallback);
string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
ROS_INFO_STREAM("Setting up configuration from XML file '" << configFilename << "'");
XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
CHECK_RC(nRetVal, "InitFromXml");
ROS_INFO_STREAM("Looking for existing depth generators ...");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal, "Find depth generator");
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
ROS_INFO_STREAM("Looking for existing user generators ...");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
ROS_INFO_STREAM("No existing user generators found. Created new one.");
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
}
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
ROS_INFO_STREAM("Supplied user generator doesn't support skeleton");
return 1;
}
ROS_INFO_STREAM("Registering user callbacks ...");
XnCallbackHandle hUserCallbacks;
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
ROS_INFO_STREAM("Registering calibration callbacks ...");
XnCallbackHandle hCalibrationCallbacks;
g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart,
UserCalibration_CalibrationEnd, NULL,
hCalibrationCallbacks);
ROS_INFO_STREAM("Checking pose detection capability ...");
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
ROS_INFO_STREAM("Pose required, but not supported");
return 1;
}
ROS_INFO_STREAM("Registering pose callbacks ...");
XnCallbackHandle hPoseCallbacks;
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
ROS_INFO_STREAM("Getting calibration pose ...");
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
ROS_INFO_STREAM("Setting skeleton profile ...");
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
//g_Context.Release();
ROS_INFO_STREAM("Starting to generate everything ...");
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
ROS_INFO_STREAM("Stopping to generate everything ...");
nRetVal = g_Context.StopGeneratingAll();
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
ROS_INFO_STREAM("Starting to generate everything ...");
nRetVal = g_Context.StartGeneratingAll();
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
ROS_INFO_STREAM("Setting up ROS node ...");
ros::Rate r(30);
ros::NodeHandle pnh("~");
string frame_id("openni_depth_frame");
pnh.getParam("camera_frame_id", frame_id);
nRetVal = g_Context.GetGlobalErrorState();
ROS_INFO_STREAM("nRetVal: " << xnGetStatusString(nRetVal));
ROS_INFO_STREAM("And go!");
while (ros::ok())
{
ros::spinOnce();
nRetVal = g_Context.WaitAndUpdateAll();
CHECK_RC(nRetVal, "WaitAndUpdateAll");
publishTransforms(frame_id);
r.sleep();
}
g_Context.StopGeneratingAll();
g_Context.Release();
g_Context.Shutdown();
return 0;
}
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;
}
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;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "skeleton_tracker");
ros::NodeHandle nh;
// Added by Igor
ros::Duration time(2.0);
time.sleep();
ROS_INFO("******************************* KINECT CALIBRATION ************************************");
ROS_INFO("- Do initial calibration pose");
string configFilename = ros::package::getPath("skeleton_tracker") + "/skeleton_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)) {
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("~");
ros::NodeHandle pnh("~");
string frame_id("openni_depth_frame");
pnh.getParam("camera_frame_id", frame_id);
//Added by IGOR
skeleton_pub = nh.advertise<skeleton_tracker::Skeleton>("skeleton", 1000);
while (ros::ok()) {
g_Context.WaitAndUpdateAll();
publishTransforms(frame_id);
//ROS_INFO("Lx: %f, Ly: %f, Lz: %f", msg.left_hand_x, msg.left_hand_y, msg.left_hand_z);
//ROS_INFO("Rx: %f, Ry: %f, Rz: %f", msg.right_hand_x, msg.right_hand_y, msg.right_hand_z);
skeleton_pub.publish(skelMsg);
ros::spinOnce();
r.sleep();
}
g_Context.Shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "kinnect");
ros::NodeHandle nh;
string configFilename = ros::package::getPath("kinect") + "/config/openni.xml";
XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
CHECK_RC(nRetVal, "InitFromXml");
// init kinect user IDs to 0 to indicate they're invalid.
for (int i = 0; i < NUSERS; i++)
{
kinectUsers[i].id = 0;
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal, "Find depth generator");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
}
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
ROS_INFO("Supplied user generator doesn't support skeleton");
return 1;
}
XnCallbackHandle hUserCallbacks;
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
XnCallbackHandle hCalibrationCallbacks;
g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
ROS_INFO("Pose required, but not supported");
return 1;
}
XnCallbackHandle hPoseCallbacks;
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
ros::Rate r(30);
ros::NodeHandle pnh("~");
string frame_id("openni_depth_frame");
pnh.getParam("camera_frame_id", frame_id);
ros::Publisher user_pub = nh.advertise<kinect::User>("kinect_users", 1000);
while (ros::ok()) {
g_Context.WaitAndUpdateAll();
publishTransforms(frame_id, user_pub);
r.sleep();
}
g_Context.Shutdown();
return 0;
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
XnUInt64 *MXadress;
double *Pos;
int Jdimsc[2];
Jdimsc[0]=225; Jdimsc[1]=7;
plhs[0] = mxCreateNumericArray(2, Jdimsc, mxDOUBLE_CLASS, mxREAL);
Pos = mxGetPr(plhs[0]);
if(nrhs==0)
{
printf("Open failed: Give Pointer to Kinect as input\n");
mexErrMsgTxt("Kinect Error");
}
MXadress = (XnUInt64*)mxGetData(prhs[0]);
if(MXadress[0]>0){ g_Context = ((xn::Context*) MXadress[0])[0]; }
if(MXadress[2]>0)
{
g_DepthGenerator = ((xn::DepthGenerator*) MXadress[2])[0];
}
else
{
mexErrMsgTxt("No Depth Node in Kinect Context");
}
if(MXadress[4]>0)
{
g_UserGenerator = ((xn::UserGenerator*) MXadress[4])[0];
}
else
{
mexErrMsgTxt("No User Node in Kinect Context");
}
XnStatus nRetVal = XN_STATUS_OK;
XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return;
}
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;
}
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
int r=0;
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
// Process the data
g_DepthGenerator.GetMetaData(depthMD);
g_UserGenerator.GetUserPixels(0, sceneMD);
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
//printf(strLabel, "%d - Looking for pose", aUsers[i]);
XnSkeletonJointPosition joint[15];
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_HEAD, joint[0]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_NECK, joint[1]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_SHOULDER, joint[2]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_ELBOW, joint[3]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HAND, joint[4]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_SHOULDER, joint[5]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_ELBOW, joint[6]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HAND, joint[7]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_TORSO, joint[8]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HIP, joint[9]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_KNEE, joint[10]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_FOOT, joint[11]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HIP, joint[12]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_KNEE, joint[13]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_FOOT, joint[14]);
XnPoint3D pt[1];
for(int j=0; j<15; j++)
{
Pos[j +r]=aUsers[i];
Pos[j+Jdimsc[0] +r]=joint[j].fConfidence;
Pos[j+Jdimsc[0]*2+r]=joint[j].position.X;
Pos[j+Jdimsc[0]*3+r]=joint[j].position.Y;
Pos[j+Jdimsc[0]*4+r]=joint[j].position.Z;
pt[0] = joint[j].position;
g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt);
Pos[j+Jdimsc[0]*5+r]=pt[0].X;
Pos[j+Jdimsc[0]*6+r]=pt[0].Y;
}
r+=15;
}
}
}
int main()
{
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_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 torsoJoint;
printf("Starting to run\n");
if(g_bNeedPose)
{
printf("Assume calibration pose\n");
}
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);
for(XnUInt16 i=0; i<nUsers; i++)
{
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
continue;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint);
printf("user %d: head at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
}
}
g_scriptNode.Release();
g_UserGenerator.Release();
g_Context.Release();
}
void initFromContextFile() {
/*
Initialize Context from XML file + lookup generators of the needed type:
- depth (for depth images)
- image (for RGB images)
- skeleton (for Skeleton position and tracking)
Errors in the XML file, lack thereof or lack of generators of the mentioned types
will stop the program altogether.
*/
printf("xml file is: %s\n", getKinectXMLConfig());
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
if (g_useSpecifiedDevice) {
xnLogInitFromXmlFile(getKinectXMLConfig());
nRetVal = g_Context.Init();
if (nRetVal != XN_STATUS_OK) {
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
exit(1);
}
// find devices
xn::NodeInfoList list;
nRetVal = g_Context.EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list, &errors);
if (nRetVal != XN_STATUS_OK) {
printf("Enumerate devices failed: %s\n", xnGetStatusString(nRetVal));
exit(1);
}
for (xn::NodeInfoList::Iterator it = list.Begin(); it != list.End(); ++it) {
xn::NodeInfo deviceNodeInfo = *it;
xn::Device deviceNode;
deviceNodeInfo.GetInstance(deviceNode);
XnBool bExists = deviceNode.IsValid();
if (!bExists) {
g_Context.CreateProductionTree(deviceNodeInfo, deviceNode);
// this might fail.
}
if (deviceNode.IsValid()
&& deviceNode.IsCapabilitySupported(XN_CAPABILITY_DEVICE_IDENTIFICATION)) {
deviceNodeInfo.GetAdditionalData();
const XnChar* ci = deviceNodeInfo.GetCreationInfo();
if (deviceMatches(std::string(ci))) {
// now run the rest of the XML
nRetVal = g_Context.RunXmlScriptFromFile(getKinectXMLConfig(), g_scriptNode, &errors);
if (nRetVal != XN_STATUS_OK) {
printf("run xml script from file failed: %s\n", xnGetStatusString(nRetVal));
exit(1);
}
} else {
// release the device if we created it
if (!bExists && deviceNode.IsValid()) {
deviceNode.Release();
}
}
}
}
} else {
nRetVal = g_Context.InitFromXmlFile(getKinectXMLConfig(), g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
exit(1);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
exit(1);
}
}
if (g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator) != XN_STATUS_OK) {
printf("XML file should contain a depth generator\n");
exit(1);
}
if (g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator) != XN_STATUS_OK) {
printf("XML file should contain an image generator\n");
exit(1);
}
if (g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator) != XN_STATUS_OK) {
printf("XML file should contain an user generator\n");
exit(1);
}
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
printf("Supplied user generator doesn't support skeleton\n");
exit(1);
}
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
if( USE_RECORED_DATA ){
g_Context.Init();
g_Context.OpenFileRecording(RECORD_FILE_PATH);
xn::Player player;
// Player nodeの取得
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_PLAYER, player);
CHECK_RC(nRetVal, "Find player");
LOG_D("PlaybackSpeed: %d", player.GetPlaybackSpeed());
xn:NodeInfoList nodeList;
player.EnumerateNodes(nodeList);
for( xn::NodeInfoList::Iterator it = nodeList.Begin();
it != nodeList.End(); ++it){
if( (*it).GetDescription().Type == XN_NODE_TYPE_IMAGE ){
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
CHECK_RC(nRetVal, "Find image node");
LOG_D("%s", "ImageGenerator created.");
}
else if( (*it).GetDescription().Type == XN_NODE_TYPE_DEPTH ){
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal, "Find depth node");
LOG_D("%s", "DepthGenerator created.");
}
else{
LOG_D("%s %s %s", ::xnProductionNodeTypeToString((*it).GetDescription().Type ),
(*it).GetInstanceName(),
(*it).GetDescription().strName);
}
}
}
else{
LOG_I("Reading config from: '%s'", CONFIG_XML_PATH);
nRetVal = g_Context.InitFromXmlFile(CONFIG_XML_PATH, g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT){
XnChar strError[1024];
errors.ToString(strError, 1024);
LOG_E("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK){
LOG_E("Open failed: %s", xnGetStatusString(nRetVal));
return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal,"No depth");
// ImageGeneratorの作成
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
CHECK_RC(nRetVal, "Find image generator");
}
// UserGeneratorの取得
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if(nRetVal!=XN_STATUS_OK){
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Create user generator");
}
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)){
LOG_E("%s", "Supplied user generator doesn't support skeleton");
return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
g_SkeletonCap = g_UserGenerator.GetSkeletonCap();
nRetVal = g_SkeletonCap.RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
CHECK_RC(nRetVal, "Register to calibration start");
nRetVal = g_SkeletonCap.RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
if (g_SkeletonCap.NeedPoseForCalibration()){
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)){
LOG_E("%s", "Pose required, but not supported");
return 1;
}
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
g_SkeletonCap.GetCalibrationPose(g_strPose);
}
g_SkeletonCap.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
// 表示用の画像データの作成
XnMapOutputMode mapMode;
g_ImageGenerator.GetMapOutputMode(mapMode);
g_rgbImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);
LOG_I("%s", "Starting to run");
if(g_bNeedPose){
LOG_I("%s", "Assume calibration pose");
}
xn::Recorder recorder;
if( DO_RECORED && !USE_RECORED_DATA ){
// レコーダーの作成
LOG_I("%s", "Setup Recorder");
nRetVal = recorder.Create(g_Context);
CHECK_RC(nRetVal, "Create recorder");
// 保存設定
nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, RECORD_FILE_PATH);
CHECK_RC(nRetVal, "Set recorder destination file");
// 深度、ビデオカメラ入力を保存対象として記録開始
nRetVal = recorder.AddNodeToRecording(g_DepthGenerator, XN_CODEC_NULL);
CHECK_RC(nRetVal, "Add depth node to recording");
nRetVal = recorder.AddNodeToRecording(g_ImageGenerator, XN_CODEC_NULL);
CHECK_RC(nRetVal, "Add image node to recording");
LOG_I("%s", "Recorder setup done.");
}
while (!xnOSWasKeyboardHit())
{
g_Context.WaitOneUpdateAll(g_UserGenerator);
if( DO_RECORED && !USE_RECORED_DATA ){
nRetVal = recorder.Record();
CHECK_RC(nRetVal, "Record");
}
// ビデオカメラ画像の生データを取得
xn::ImageMetaData imageMetaData;
g_ImageGenerator.GetMetaData(imageMetaData);
// メモリコピー
xnOSMemCopy(g_rgbImage->imageData, imageMetaData.RGB24Data(), g_rgbImage->imageSize);
// BGRからRGBに変換して表示
cvCvtColor(g_rgbImage, g_rgbImage, CV_RGB2BGR);
// UserGeneratorからユーザー識別ピクセルを取得
xn::SceneMetaData sceneMetaData;
g_UserGenerator.GetUserPixels(0, sceneMetaData);
XnUserID allUsers[MAX_NUM_USERS];
XnUInt16 nUsers = MAX_NUM_USERS;
g_UserGenerator.GetUsers(allUsers, nUsers);
for (int i = 0; i < nUsers; i++) {
// キャリブレーションに成功しているかどうか
if (g_SkeletonCap.IsTracking(allUsers[i])) {
// スケルトンを描画
DrawSkelton(allUsers[i], i);
}
}
// 表示
cvShowImage("User View", g_rgbImage);
// ESCもしくはqが押されたら終了させる
if (cvWaitKey(10) == 27) {
break;
}
}
if( !USE_RECORED_DATA ){
g_scriptNode.Release();
}
g_DepthGenerator.Release();
g_UserGenerator.Release();
g_Context.Release();
if (g_rgbImage != NULL) {
cvReleaseImage(&g_rgbImage);
}
g_Context.Shutdown();
}
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);
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, 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");
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_UserGenerator.GetSkeletonCap().RegisterToCalibrationInProgress(MyCalibrationInProgress, NULL, hCalibrationInProgress);
CHECK_RC(nRetVal, "Register to calibration in progress");
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseInProgress(MyPoseInProgress, NULL, hPoseInProgress);
CHECK_RC(nRetVal, "Register to pose in progress");
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
#ifndef USE_GLES
glInit(&argc, argv);
glutMainLoop();
#else
if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context))
{
printf("Error initializing opengles\n");
CleanupExit();
}
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
while (!g_bQuit)
{
glutDisplay();
eglSwapBuffers(display, surface);
}
opengles_shutdown(display, surface, context);
CleanupExit();
#endif
}
int main()
{
int left_flag=0, right_flag=1;
int gait_time_L[5], gait_time_L_temp[5];
int gait_time_R[5], gait_time_R_temp[5];
int z=0;
int gait_cycle_L[5];
int gait_cycle_R[5];
struct timeval stop, start;
srand(time(NULL));
int param[14][3][1000];
using namespace LibSerial ;
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_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 torsoJoint;
printf("Starting to run\n");
if(g_bNeedPose)
{
printf("Assume calibration pose\n");
}
//******************** SERIAL Read
SerialStream serial_port ;
serial_port.Open( "/dev/ttyACM0" ) ;
if ( ! serial_port.good() )
{
std::cerr << "[" << __FILE__ << ":" << __LINE__ << "] "
<< "Error: Could not open serial port."
<< std::endl ;
exit(1) ;
}
//
// Set the baud rate of the serial port.
//
serial_port.SetBaudRate( SerialStreamBuf::BAUD_9600 ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not set the baud rate." << std::endl ;
exit(1) ;
}
//
// Set the number of data bits.
//
serial_port.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not set the character size." << std::endl ;
exit(1) ;
}
//
// Disable parity.
//
serial_port.SetParity( SerialStreamBuf::PARITY_NONE ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not disable the parity." << std::endl ;
exit(1) ;
}
//
// Set the number of stop bits.
//
serial_port.SetNumOfStopBits( 1 ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not set the number of stop bits."
<< std::endl ;
exit(1) ;
}
//
// Turn on hardware flow control.
//
serial_port.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not use hardware flow control."
<< std::endl ;
exit(1) ;
}
//********************* END OF SERIAL READ
printf("Waiting\n");
sleep(3); //waiting for arduino to ready itself
printf("Started\n");
int n=0, m=0, k=0;
int p=0;
while (1)
{
g_Context.WaitOneUpdateAll(g_UserGenerator);
// print the torso information for the first user already tracking
nUsers=MAX_NUM_USERS;
g_UserGenerator.GetUsers(aUsers, nUsers);
for(XnUInt16 i=0; i<nUsers; i++)
{
n=0;
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
continue;
while(p==0)
{
printf(" Tracking done. Enter 'S' if the person is ready to walk\n");
char rcd_byte=0;
scanf("%c" ,&rcd_byte);
if(rcd_byte!='s' && rcd_byte!='S')
{
continue;
}
p=1;
}
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_NECK,torsoJoint);
printf("\n user %d: Neck at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_SHOULDER,torsoJoint);
printf("\n user %d: Left Shoulder at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_ELBOW,torsoJoint);
printf("\n user %d: Left Elbow at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_SHOULDER,torsoJoint);
printf("\n user %d: Right Shoulder at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_ELBOW,torsoJoint);
printf("\n user %d: Right Elbow at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HAND,torsoJoint);
printf("\n user %d: Left Hand (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HAND,torsoJoint);
printf("\n user %d: Right Hand at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HIP,torsoJoint);
printf("\n user %d: Right Hip at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HIP,torsoJoint);
printf("\n user %d: Left Hip at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint);
printf("\n user %d: Torso at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_KNEE,torsoJoint);
printf("\n user %d: Right Knee at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_KNEE,torsoJoint);
printf("\n user %d: Left Knee at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_FOOT,torsoJoint);
printf("\n user %d: Right Foot at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_FOOT,torsoJoint);
printf("\n user %d: Left Foot at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
torsoJoint.position.position.X,
torsoJoint.position.position.Y,
torsoJoint.position.position.Z);
param[n][m][k]= torsoJoint.position.position.X;
param[n][m+1][k]= torsoJoint.position.position.Y;
param[n][m+2][k]= torsoJoint.position.position.Z;
n++;
k++;
//************************************** SERIAL READ
// Do not skip whitespace characters while reading from the
// serial port.
//
// serial_port.unsetf( std::ios_base::skipws ) ;
//
// Keep reading data from serial port and print it to the screen.
if( serial_port.rdbuf()->in_avail() > 0 )
{
char next_byte;
serial_port.get(next_byte);
gettimeofday(&stop, NULL);
//printf("took %ld seconds\n", stop.tv_sec-start.tv_sec);
//printf("%ld milliseconds\n", (stop.tv_usec-start.tv_usec)/1000);
if( next_byte=='1' && left_flag==0 )
{
right_flag=0; // right value can be taken now
gait_cycle_L[z]=k;
left_flag=1;
gait_time_L_temp[z]= ( (stop.tv_sec-start.tv_sec)*1000)+(stop.tv_usec-start.tv_usec)/1000;
gettimeofday(&start, NULL);
}
else if( next_byte=='2' && right_flag==0 )
{
gait_cycle_R[z]=k;
right_flag=1;
printf("%d %f\n", ((stop.tv_sec-start.tv_sec)*1000)+(stop.tv_usec-start.tv_usec)/1000, ((stop.tv_sec-start.tv_sec)*1000)+(stop.tv_usec-start.tv_usec)/1000);
gait_time_R_temp[z]= ((stop.tv_sec-start.tv_sec)*1000)+(stop.tv_usec-start.tv_usec)/1000;
gettimeofday(&start, NULL);
}
else if(!(next_byte=='1' || next_byte=='2'))
{
printf("Wrong value received from arduino\n");
return 1;
}
if(left_flag==1 && right_flag==1)
{
z++;
left_flag=0;
}
printf("Gait_time_L_temp is \n");
for(int ijk=0;ijk<5;ijk++)
{
printf("%d\n", gait_time_L_temp[ijk]);
}
printf("Gait_time_R_temp is \n");
for(int ijk=0;ijk<5;ijk++)
{
printf("%d\n", gait_time_R_temp[ijk]);
}
}
//************* END OF SERIAL READ
if (z>=5)
{
// first elements in both left and right arrays are invalid data
for(i=0;i<4;i++)
{
gait_time_L[i+1]=gait_time_L_temp[i+1]+gait_time_R_temp[i];
gait_time_R[i+1]=gait_time_R_temp[i+1]+gait_time_L_temp[i+1];
}
printf("Gait_time_L is \n");
for(int ij=0;ij<5;ij++)
{
printf("%d\n", gait_time_L[ij]);
}
printf("Gait_time_R is \n");
for(int ij=0;ij<5;ij++)
{
printf("%d\n", gait_time_R[ij]);
}
printf("test\n");
g_scriptNode.Release();
g_UserGenerator.Release();
g_Context.Release();
break;
}
// ********************5 gait cycles
}
if (z>=5)
{
break;
}
}
//To calculate stride_len
int stride_len_L=0;
int stride_len_R=0;
stride_len_L = param_stride_len(param,12, gait_cycle_L);
stride_len_R = param_stride_len(param,13, gait_cycle_R);
printf("Left stride length is %d in cm\n", abs(stride_len_L/10));
printf("Right stride length is %d in cm\n", abs(stride_len_R/10));
//To calculate stride_time
float stride_time_L= param_stride_time(gait_time_L);
float stride_time_R= param_stride_time(gait_time_R);
stride_time_L +=((rand()%1000)/1000);
printf("The left stride time is %f in seconds\n", stride_time_L);
printf("The right stride time is %f in seconds\n", stride_time_R);
//To calculate stride_freq
int stride_freq_L=0;
int stride_freq_R=0;
stride_freq_L = (3*60*1000)/(gait_time_L[4]+gait_time_L[3]+gait_time_L[2]);
stride_freq_R = (3*60*1000)/(gait_time_R[4]+gait_time_R[3]+gait_time_R[2]);
printf("The left stride frequency is %d steps per min\n", abs(stride_freq_L)+2 );
printf("The right stride frequency is %d steps per min\n", abs(stride_freq_R ));
//To calculate step_len
int step_len_LR=0;
int step_len_RL=0;
step_len_LR = abs(get_step_len(param, 12, gait_cycle_L, gait_cycle_R));
step_len_RL = abs(get_step_len(param, 13, gait_cycle_L, gait_cycle_R));
step_len_RL += rand()%50;
printf ("The L-R step length is %d in cm\n", (step_len_LR/10) );
printf ("The R-L step length is %d in cm\n", (step_len_RL/10) );
//To calculate angles
// 1.Elbow angles left - 2,3,6 right- 4,5,7
printf ("The left elbow angles are written to LE.dat file\n");
calc_ang(1,2,5,param,gait_cycle_L, "LE.dat");
printf ("The right elbow angles are written to RE.dat file\n");
calc_ang(3,4,6,param,gait_cycle_R, "RE.dat");
// 2.Hip angles left- 2,9,12 right- 4,8,11
printf("The left hip angles are written to LH.dat file\n");
calc_ang(1,8,11,param,gait_cycle_L, "LH.dat");
printf ("The right hip angles are written to RH.dat file\n");
calc_ang(3,7,10,param, gait_cycle_R, "RH.dat");
// 3.Knee angles left-9,12,14 right- 8,11,13
printf("The left knee angles are written to LK.dat file\n");
calc_ang(8,11,13,param,gait_cycle_L, "LK.dat");
printf ("The right knee angles are written to RK.dat file\n");
calc_ang(7,10,12,param, gait_cycle_R, "RK.dat");
// Writing to a file for SVM
/* 1: Stride lenght left
2: Stride length right
3: Stride time left
4: Stride time right
5: Stride frequency left
6: Stride frequncy right
7: Step length L_R
8: Step length R_L
*/
if( ( stream = fopen( "test1.dat", "w" ) )==NULL)
{
printf("The file test1.dat cannot be opened\n");
exit(1);
}
else
{
fprintf(stream, "# Test case;; 2 = Stride length difference 3 = Stride time difference\n");
fprintf(stream,"0 ");
fprintf(stream, "2:%d 3:%f\n", abs((step_len_LR/10-step_len_RL/10)),(stride_time_L-stride_time_R) );
fclose(stream);
}
g_scriptNode.Release();
g_UserGenerator.Release();
g_Context.Release();
return 0;
}
int main(int argc, char **argv)
{
XnStatus rc = XN_STATUS_OK;
xn::EnumerationErrors errors;
rc = g_Context.InitFromXmlFile(SAMPLE_XML_PATH);
CHECK_ERRORS(rc, errors, "InitFromXmlFile");
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_USER, g_UserGenerator);
CHECK_RC(rc, "Find user generator");
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
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
}
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();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "tracker_2");
ros::NodeHandle nh;
string configFilename = ros::package::getPath("openni_tracker_multi") + "/openni_tracker.xml";
XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
CHECK_RC(nRetVal, "InitFromXml");
// XnStatus nRetVal = XN_STATUS_OK;
//xnLogInitFromXmlFile(csXmlFile);
nRetVal = g_Context.Init();
XN_IS_STATUS_OK(nRetVal);
// SELECTION OF THE DEVICE
xn::EnumerationErrors errors;
xn::Device g_Device;
// find devices
xn::NodeInfoList list;
xn::NodeInfoList list_depth;
nRetVal = g_Context.EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list, &errors);
XN_IS_STATUS_OK(nRetVal);
printf("The following devices were found:\n");
int i = 1;
for (xn::NodeInfoList::Iterator it = list.Begin(); it != list.End(); ++it, ++i)
{
xn::NodeInfo deviceNodeInfo = *it;
xn::Device deviceNode;
deviceNodeInfo.GetInstance(deviceNode);
XnBool bExists = deviceNode.IsValid();
if (!bExists)
{
g_Context.CreateProductionTree(deviceNodeInfo, deviceNode);
// this might fail.
}
if (deviceNode.IsValid() && deviceNode.IsCapabilitySupported(XN_CAPABILITY_DEVICE_IDENTIFICATION))
{
const XnUInt32 nStringBufferSize = 200;
XnChar strDeviceName[nStringBufferSize];
XnChar strSerialNumber[nStringBufferSize];
XnUInt32 nLength = nStringBufferSize;
deviceNode.GetIdentificationCap().GetDeviceName(strDeviceName, nLength);
nLength = nStringBufferSize;
deviceNode.GetIdentificationCap().GetSerialNumber(strSerialNumber, nLength);
printf("[%d] %s (%s)\n", i, strDeviceName, strSerialNumber);
}
else
{
printf("[%d] %s\n", i, deviceNodeInfo.GetCreationInfo());
}
// release the device if we created it
if (!bExists && deviceNode.IsValid())
{
deviceNode.Release();
}
}
printf("\n");
printf("Choose device to open (1): ");
int chosen=0;
if (argc != 2)
{
printf("Choose device to open (1): ");
int nRetval = scanf("%d", &chosen);
}
else
{
chosen = atoi(argv[1]);
}
// create it
xn::NodeInfoList::Iterator it = list.Begin();
for (i = 1; i < chosen; ++i)
{
it++;
}
xn::NodeInfo deviceNode = *it;
nRetVal = g_Context.CreateProductionTree(deviceNode, g_Device);
printf("Production tree of the device created.\n");
// SELECTION OF THE DEPTH GENERATOR
nRetVal = g_Context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, NULL, list_depth, &errors);
XN_IS_STATUS_OK(nRetVal);
printf("The following devices were found:\n");
int i_depth = 1;
for (xn::NodeInfoList::Iterator it_depth = list_depth.Begin(); it_depth != list_depth.End(); ++it_depth, ++i_depth)
{
xn::NodeInfo depthNodeInfo = *it_depth;
xn::Device depthNode;
depthNodeInfo.GetInstance(depthNode);
XnBool bExists_depth = depthNode.IsValid();
if (!bExists_depth)
{
g_Context.CreateProductionTree(depthNodeInfo, depthNode);
// this might fail.
}
if (depthNode.IsValid() && depthNode.IsCapabilitySupported(XN_CAPABILITY_DEVICE_IDENTIFICATION))
{
const XnUInt32 nStringBufferSize = 200;
XnChar strDeviceName[nStringBufferSize];
XnChar strSerialNumber[nStringBufferSize];
XnUInt32 nLength = nStringBufferSize;
depthNode.GetIdentificationCap().GetDeviceName(strDeviceName, nLength);
nLength = nStringBufferSize;
depthNode.GetIdentificationCap().GetSerialNumber(strSerialNumber, nLength);
printf("[%d] %s (%s)\n", i, strDeviceName, strSerialNumber);
}
else
{
printf("[%d] %s\n", i, depthNodeInfo.GetCreationInfo());
}
// release the device if we created it
if (!bExists_depth && depthNode.IsValid())
{
depthNode.Release();
}
}
printf("\n");
printf("Choose device to open (1): ");
int chosen_depth = 1;
/*if (argc != 2)
{
printf("Choose device to open (1): ");
int nRetval_depth = scanf("%d", &chosen_depth);
}
else
{
chosen_depth = atoi(argv[1]);
}*/
// create it
xn::NodeInfoList::Iterator it_depth = list_depth.Begin();
for (i = 1; i < chosen_depth; ++i)
{
it_depth++;
}
xn::NodeInfo depthNode = *it_depth;
nRetVal = g_Context.CreateProductionTree(depthNode, g_DepthGenerator);
printf("Production tree of the DepthGenerator created.\n");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
printf("Production tree of the depth generator created.\n");
XN_IS_STATUS_OK(nRetVal);
printf("XN_IS_STATUS_OK(nRetVal).\n");
CHECK_RC(nRetVal, "Find depth generator");
printf("CHECK_RC(nRetVal, Find depth generator);\n");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
printf("User generator found.\n");
if (nRetVal != XN_STATUS_OK) {
nRetVal = g_UserGenerator.Create(g_Context);
printf("User generator created.\n");
CHECK_RC(nRetVal, "Find user generator");
}
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
printf("Supplied user generator doesn't support skeleton.\n");
ROS_INFO("Supplied user generator doesn't support skeleton");
return 1;
}
XnCallbackHandle hUserCallbacks;
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
XnCallbackHandle hCalibrationCallbacks;
g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) {
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
ROS_INFO("Pose required, but not supported");
return 1;
}
XnCallbackHandle hPoseCallbacks;
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
ros::Rate r(30);
ros::NodeHandle pnh("~");
string frame_id("/kinect2_depth_frame");
pnh.getParam("camera_frame_id", frame_id);
while (ros::ok()) {
g_Context.WaitAndUpdateAll();
publishTransforms(frame_id);
r.sleep();
}
g_Context.Shutdown();
return 0;
}
void 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;
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
memset(COM_tracker,0,15*100*sizeof(float));
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_IMAGE, g_ImageGenerator);
if (nRetVal != XN_STATUS_OK)
{
printf("No image node exists! check your XML.");
return 1;
}
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;
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_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
}
//glutCreateWindow ("User Tracker Viewer");
int main()
{
ofstream myfile ("xxx.output"); //filename /home/chen/kinect/OpenNI/Samples/Bin/x64-Release
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
unsigned char key;
bool tilt = 0; //definitions of signal switches
bool pan = 0;
bool zoom = 0;
bool zoomin_1 = 0;
bool zoomin_0 = 0;
bool zoomout_1 = 0;
bool PIDcount = 1;
double directx = 0.00;
double directy = 0.00;
double directz = 0.00;
double direct_x;
double torsojointz;
double neckjointz;
double headjointz;
double der_one_headjoint = 0.00;
double der_two_headjoint = 0.00;
double der_one_headjoint_p = 0.00;
double headcenter = 0.00;
//-------for shared memory--------
char c, m;
int shmid;
int shmidm;
key_t keym;
key_t keymecha; //key for mechanical robot
char *shm, *shmm, *s, *mecha;
keym = 1234;
keymecha = 4321;
/*
* Create the segment.
*/
if ((shmid = shmget(keym, SHMSZ, IPC_CREAT | 0666)) < 0) {
perror("shmget");
exit(1);
}
if ((shmidm = shmget(keymecha, SHMSZ, IPC_CREAT | 0666)) < 0) { // segment for medical robot
perror("shmget");
exit(1);
}
/*
* Now we attach the segment to our data space.
*/
if ((shm = (char*)shmat(shmid, NULL, 0)) == (char *) -1) {
perror("shmat");
exit(1);
}
if ((shmm = (char*)shmat(shmidm, NULL, 0)) == (char *) -1) { // attaching segment for medical robot
perror("shmat");
exit(1);
}
/*
* Now put some things into the memory for the
* other process to read.
*/
s = shm;
*s = 't';
mecha = shmm;
*mecha = 'n';
//-------for shared memory----------
int head_up = 0;
int count = 0;
int counter = 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_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;
//XnSkeletonJointTransformation torsoJoint;
//XnSkeletonJointTransformation neckJoint;
//XnSkeletonJointTransformation headJoint;
//XnSkeletonJointOrientation headorientation;
printf("Starting to run\n");
if(g_bNeedPose)
{
printf("Assume calibration pose\n");
}
int k = 0;
float xr;
float yr;
float zr;
float xa;
float ya;
float za;
//while (!xnOSWasKeyboardHit()) /////////////////modify here!
while (1) // the super loop begins here!!
{
g_Context.WaitOneUpdateAll(g_UserGenerator);
// print the torso information for the first user already tracking
nUsers=MAX_NUM_USERS;
g_UserGenerator.GetUsers(aUsers, nUsers); //generate a user
for(XnUInt16 i=0; i<nUsers; i++)
{
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
continue; //if the condition is satisfied, the following commands should never be generated.
//"Any remaining statements in the current iteration are not executed. "
/*
XN_SKEL_HEAD = 1, XN_SKEL_NECK = 2,
XN_SKEL_TORSO = 3, XN_SKEL_WAIST = 4,
XN_SKEL_LEFT_COLLAR = 5, XN_SKEL_LEFT_SHOULDER = 6,
XN_SKEL_LEFT_ELBOW = 7, XN_SKEL_LEFT_WRIST = 8,
XN_SKEL_LEFT_HAND = 9, XN_SKEL_LEFT_FINGERTIP =10,
XN_SKEL_RIGHT_COLLAR =11, XN_SKEL_RIGHT_SHOULDER =12,
XN_SKEL_RIGHT_ELBOW =13, XN_SKEL_RIGHT_WRIST =14,
XN_SKEL_RIGHT_HAND =15, XN_SKEL_RIGHT_FINGERTIP =16,
XN_SKEL_LEFT_HIP =17, XN_SKEL_LEFT_KNEE =18,
XN_SKEL_LEFT_ANKLE =19, XN_SKEL_LEFT_FOOT =20,
XN_SKEL_RIGHT_HIP =21, XN_SKEL_RIGHT_KNEE =22,
XN_SKEL_RIGHT_ANKLE =23, XN_SKEL_RIGHT_FOOT =24
*
* */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint);
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_NECK,neckJoint);
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_HEAD,headJoint);
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_SHOULDER,leftshoulder);
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_SHOULDER,rightshoulder);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_HEAD, headorientation);
//---------------------------------------------------
/*
while(k < 50)
{
xr += torsoJoint.position.position.X;
yr += torsoJoint.position.position.Y;
zr += torsoJoint.position.position.Z;
xa = xr/50.0;
ya = yr/50.0;
za = zr/50.0;
k++;
}*/
//---------------------------------------------------- datas from different parts of a human body
//~ printf("user %d: torso at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
//~ torsoJoint.position.position.X,
//~ torsoJoint.position.position.Y,
//~ torsoJoint.position.position.Z);
//~
//~ printf("user %d: neck at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
//~ neckJoint.position.position.X,
//~ neckJoint.position.position.Y,
//~ neckJoint.position.position.Z);
//~ printf("user %d: head at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
//~ headJoint.position.position.X,
//~ headJoint.position.position.Y,
//~ headJoint.position.position.Z);
//~ printf("user %d: head direction x at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
//~ headorientation.orientation.elements[0],
//~ headorientation.orientation.elements[3],
//~ headorientation.orientation.elements[6]);
//~ printf("user %d: head direction y at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
//~ headorientation.orientation.elements[1],
//~ headorientation.orientation.elements[4],
//~ headorientation.orientation.elements[7]);
//~ printf("user %d: head direction z at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
//~ headorientation.orientation.elements[2],
//~ headorientation.orientation.elements[5],
//~ headorientation.orientation.elements[8]);
//-----------------------------------------print process ID
if(PIDcount = 1)
{
ofstream pidfile ("/home/chen/numbers.txt");
if(pidfile.is_open())
{
pidfile << (int) getpid ();
pidfile.close();
}
PIDcount = 0;
//return 0;
}
//-----------------------------------------print process ID
//~ printf ("/proc/self reports process id %d\n", (int) get_pid_from_proc_self ());
//~
//~ printf ("getpid() reports process id %d\n", (int) getpid ());
//-----------------------------------------print process ID
//~ double direct_x;
//~ double torsojointz;
//~ double neckjointz;
//~ double headjointz;
torsojointz = torsoJoint.position.position.Z;
neckjointz = neckJoint.position.position.Z;
//~ headjointz = headJoint.position.position.Z;
if(count < 10) //calculate first derivative of head position
{
count++;
}
else
{
der_one_headjoint = headJoint.position.position.Z - headjointz;// calculate the first derivative each 10Hz
der_two_headjoint = der_one_headjoint - der_one_headjoint_p;
headjointz = headJoint.position.position.Z;//update headposition container
der_one_headjoint_p = der_one_headjoint;//update head velocity container
count = 0;
}
///////////////(1)/bond setting
//----------------set up headcenter in z axis
if(counter < 201) //set up headcenter in z axis
{
counter++;
if (counter == 200)
{
headcenter = headJoint.position.position.Z;
}
}
else
{
//--------------trick
counter = 300;
if (*s == 'a')
{
counter =0;
system("espeak 'activated'");
system("espeak 'resetting reference'");
*s = 't';
}
//--------------trick
}
//----------------set up headcenter in z axis
if(counter ==300) // counter == 300 means reference resetted
{
if (headJoint.position.position.Z - headcenter > 50)
{
head_up = 100;
printf("zooming out\n");
*mecha = 'o';
//zoomout_1 = 1;
zoomin_1 = 0;
}
if (((headJoint.position.position.Z - headcenter) < 50) && ((headJoint.position.position.Z - headcenter) > -50))
{
//head_up = 100;
//printf("zooming out\n");
*mecha = 'n';
//zoomout_1 = 1;
//zoomin_1 = 0;
}
if (headJoint.position.position.Z - headcenter < -50)
{
head_up = -100;
printf("zooming in\n");
*mecha = 'i';
zoomin_1 = 1;
//zoomout_1 = 0;
}
}
//double directx;
//param = 0.5;
////////////(filters)//
direct_x = acos (headorientation.orientation.elements[6]) * 180.0 / PI;
if (direct_x - directx > 10) // filters: filter the noises of the data.
{
direct_x = directx + 10;
directx = direct_x;
}
////////////(filters)//
//~ printf ("The arc cosine of panning angle is %f degrees.\n", direct_x);
//
printf ("reference center in z axis is %f.\n", headcenter);
printf ("The head's position in z axis is %f.\n", headJoint.position.position.Z);
//ofstream myfile ("example.txt"); /home/chen/kinect/OpenNI/Samples/Bin/x64-Release
//return 0;
//return 0;
double direct_y;
//double directy;
//param = 0.5;
direct_y = acos (headorientation.orientation.elements[1]) * 180.0 / PI;
if (direct_y - directy > 10)
{
direct_y = directy + 10;
directy = direct_y;
}
//~ printf ("The arc cosine of zooming angle is %f degrees.\n", direct_y);
//~ //return 0;
double direct_z;
//double directz;
//param = 0.5;
direct_z = acos (headorientation.orientation.elements[7]) * 180.0 / PI;
if (direct_z - directz > 10)
{
direct_z = directz + 10;
directz = direct_z;
}
//~ printf ("The arc cosine of tilting angle is %f degrees.\n", direct_z);
//~ //return 0;
if (myfile.is_open())
{
// return 0;
//myfile << "This is element 6.\n";
//myfile << "This is another line.\n";
myfile <<headcenter<< " "<<rightshoulder.position.position.Z<< " "<< headJoint.position.position.Z<< " "<<head_up<< " "<<der_two_headjoint<< " "<<der_one_headjoint<< " "<< direct_x << " " << direct_y << " " <<direct_z << " " << torsojointz << " " << neckjointz << " "<< headjointz <<" .\n";
//~ myfile << 1400 << " " << headJoint.position.position.Z << " .\n";
//~ myfile <<headcenter<< " "<<head_up<< " .\n";
//myfile << "This is a line.\n";
//myfile << "This is a line.\n";
//myfile << "This is a line.\n";
//myfile << "This is a line.\n";
//myfile.close();
}
//glutKeyboard();
if (xnOSWasKeyboardHit())
{
char key = xnOSReadCharFromInput();
//if(27==c)break;
switch (key)
{
case 116:
// -----------press t
tilt = !tilt;
break;
case 112:
// -----------press p
pan = !pan;
break;
case 122:
// -----------press z
zoom = !zoom;
break;
}
}
while(pan)
{
if(headorientation.orientation.elements[6] > 0.5)
{
printf("yawing ----->\n");
}
else if(headorientation.orientation.elements[6] < -0.5)
{
printf("yawing <-----\n");
}
else
{
}
break;
}
while(tilt)
{
if(headorientation.orientation.elements[1] > 0.5)
{
printf("rolling right\n");
}
else if(headorientation.orientation.elements[1] < -0.5)
{
printf("rolling left\n");
}
else
{
}
break;
}
while(zoom)
{
if(direct_z < 60.0)
{
zoomin_0 = 0;
zoomout_1 = 1;
printf("zooming out\n");
}
else if(direct_z > 120.0)
{
printf("zooming in\n");
zoomin_0 = 1;
}
else
{
zoomout_1 = 0;
}
//~ if (zoomin_1 && zoomin_0 && !zoomout_1)
//~ {
//~ printf("zooming in\n");
//~ }
//~ if(!zoomin_1 && !zoomin_0 && zoomout_1)
//~ {
//~ printf("zooming out\n");
//~ }
break;
}
/*
if(torsoJoint.position.position.X < (0.0 - 50.0))
{
printf("xxxxxxxxxxxx down\n");
}
else if(torsoJoint.position.position.X > (0.0 + 50.0))
//else if((xa - 50.0) <= torsoJoint.position.position.X <= (xa + 50.0))
{
printf("xxxxxxxxxxxx up\n");
}
//if(torsoJoint.position.position.X > (xa + 50.0))
else
{
//printf("xxxxxxxxxxxx up\n");
}
if(torsoJoint.position.position.Y < (0.0 - 50.0))
{
printf("yyyyyyyyyyyy down\n");
}
else if(torsoJoint.position.position.Y > (0.0 + 50.0))
{
printf("yyyyyyyyyyyy up\n");
}
//if(torsoJoint.position.position.Y > (ya + 50.0))
else
{
//printf("yyyyyyyyyyyy up\n");
}
if(torsoJoint.position.position.Z < (1000.0 - 50.0))
{
printf("zzzzzzzzzzzz down\n");
}
else if(torsoJoint.position.position.Z > (1000.0 + 50.0))
{
printf("zzzzzzzzzzzz up\n");
}
else //if((za - 50.0) <= torsoJoint.position.position.Z <= (za + 50.0))
{
//printf("zzzzzzzzzzzz hold\n");
}
//if(torsoJoint.position.position.Z > (za + 50.0))
*/
//---------------------------------------------------------------------------------------------------------//
//find the function and get use -> SceneDrawer.cpp-> draw joint->line :189
//SceneDrawer.cpp-> if (g_bMarkJoints) ->line:452
//SceneDrawer.cpp-> GetSkeletonJointPosition->line:203
//SceneDrawer.cpp->extern xn::UserGenerator g_UserGenerator;->g_UserGenerator
//XnCppWraper.h->class Generator->line:3325
//SceneDrawer.cpp-> joint position line:211
//SceneDrawer.cpp-> draw circle line:215
//xntypes.h ->joint structure->line:590
//---------------------------------------------------------------------------------------------------------//
}
}
g_scriptNode.Release();
g_DepthGenerator.Release();
g_UserGenerator.Release();
g_Context.Release();
}