void XN_CALLBACK_TYPE
Pose_Detected(xn::PoseDetectionCapability& pose, const XnChar* strPose, XnUserID nId, void* pCookie)
{
printf("Pose %s for user %d\n", strPose, nId);
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
// Callback: Finished calibration
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
if (bSuccess)
{
// Calibration succeeded
printf("Calibration complete, start tracking user %d\n", nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
prlite_kinematics::some_status_thing status;
status.lulz = 1; //calibration ok
yourmom.publish(status);
}
else
{
// Calibration failed
printf("Calibration failed for user %d\n", nId);
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
}
// Callback: New user was detected
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
XnUserID aUsers[MAX_NUM_USERS];
XnUInt16 nUsers;
g_UserGenerator.GetUsers(aUsers, nUsers);
printf("%d New User %d\n", epochTime, nId);
// New user found
//validate the number of users
XnUInt16 usersTracked=0;
for(XnUInt16 i=0; i<nUsers; i++)
{
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==TRUE)
usersTracked++;
}
if(usersTracked >= numOfUser)
{
printf("%d users are currently being tracked, no more users allowed\n",numOfUser);
return;
}
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
void XN_CALLBACK_TYPE xn_onPoseDetected
(xn::PoseDetectionCapability &cap, const XnChar* pose, XnUserID id, void *cookie)
{
fprintf(stderr, "%s detected. strat calibration\n", pose);
gUserGenerator.GetPoseDetectionCap().StopPoseDetection(id);
gUserGenerator.GetSkeletonCap().RequestCalibration(id, TRUE);
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
if (argc > 1)
{
nRetVal = g_Context.Init();
CHECK_RC(nRetVal, "Init");
nRetVal = g_Context.OpenFileRecording(argv[1]);
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();
}
// Callback: Detected a pose
void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie){
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
LOG_D("%d Pose %s detected for user %d", epochTime, strPose, nId);
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
g_SkeletonCap.RequestCalibration(nId, TRUE);
}
XnStatus hiKinect::init(const FB::JSObjectPtr &cb){
XnStatus nRetVal = XN_STATUS_OK;
callbackPtr = &cb;
nRetVal = context.Init();
(*callbackPtr)->InvokeAsync("", FB::variant_list_of("CONTEXT_INIT")
(nRetVal)(xnGetStatusString(nRetVal)));
if (nRetVal != XN_STATUS_OK) return nRetVal;
// Create the user generator
nRetVal = g_UserGenerator.Create(context);
(*callbackPtr)->InvokeAsync("", FB::variant_list_of("USERGENERATOR_CREATE")
(nRetVal)(xnGetStatusString(nRetVal)));
if (nRetVal != XN_STATUS_OK) return nRetVal;
XnCallbackHandle h1, h2, h3;
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, this, h1);
g_UserGenerator.GetPoseDetectionCap()
.RegisterToPoseCallbacks(Pose_Detected, NULL, this, h2);
g_UserGenerator.GetSkeletonCap()
.RegisterCalibrationCallbacks(Calibration_Start, Calibration_End,
this, h3);
// Set the profile
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
FBLOG_INFO("contextInit", "Init finish");
// Start generating
nRetVal = context.StartGeneratingAll();
(*callbackPtr)->InvokeAsync("", FB::variant_list_of("START_GENERATING")
(nRetVal)(xnGetStatusString(nRetVal)));
if (nRetVal != XN_STATUS_OK) return nRetVal;
while (getTrackedUserID() == 0){
contextWaitAndUpdateAll();
}
return nRetVal;
}
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
if (bSuccess)
{
ROS_INFO("Calibration complete, start tracking user %d", nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
if (default_user == 0) // set default user if still unset
{
default_user = nId;
ROS_INFO_STREAM("OpenNI tracker: Default user has been initialised with user " << default_user << ".");
}
std_msgs::UInt16MultiArray tracked_users;
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (unsigned int user = 0; user < users_count; ++user)
{
tracked_users.data.push_back(users[user]);
}
available_tracked_users_pub.publish(tracked_users);
}
else
{
ROS_INFO("Calibration failed for user %d", nId);
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
}
void XN_CALLBACK_TYPE UserCalibration_CalibrationComplete(xn::SkeletonCapability& capability, XnUserID nId, XnCalibrationStatus eStatus, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
if (eStatus == XN_CALIBRATION_STATUS_OK)
{
// Calibration succeeded
printf("%d Calibration complete, start tracking user %d\n", epochTime, nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else
{
// Calibration failed
printf("%d Calibration failed for user %d\n", epochTime, nId);
if(eStatus==XN_CALIBRATION_STATUS_MANUAL_ABORT)
{
printf("Manual abort occured, stop attempting to calibrate!");
return;
}
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
}
void XN_CALLBACK_TYPE
User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
printf("New User: %d\n", nId);
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(POSE_TO_USE,
nId);
}
// Load calibration from file
bool LoadCalibration( xn::UserGenerator& userGenerator )
{
printf("LoadCalibration\n");
XnUserID aUserIDs[20] = {0};
XnUInt16 nUsers = 20;
userGenerator.GetUsers(aUserIDs, nUsers);
for (int i = 0; i < nUsers; ++i) {
// ユーザーがキャリブレーションされているときは何もしない
if (userGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i]))
continue;
if (userGenerator.GetSkeletonCap().IsCalibrating(aUserIDs[i]))
continue;
// ファイルから、キャリブレーション情報を取得し、骨格の追跡を開始する
printf("Load user's calibration from file: user %d\n", aUserIDs[i]);
XnStatus rc = userGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
if (rc == XN_STATUS_OK) {
printf("Make sure state is coherent: user %d\n", aUserIDs[i]);
userGenerator.GetPoseDetectionCap().StopPoseDetection(aUserIDs[i]);
userGenerator.GetSkeletonCap().StartTracking(aUserIDs[i]);
return true;
}
break;
}
return false;
}
void XN_CALLBACK_TYPE whu_NewUser(xn::UserGenerator &generator,
XnUserID user,
void* pCookie)
{
cout<<"New User Identified:"<<user<<endl;
//generator.GetSkeletonCap().RequestCalibration(user,true);//假如不需要做出投降姿势才开始的话，把此句解注释
generator.GetPoseDetectionCap().StartPoseDetection("Psi", user);//做出[ 。]姿势以开始用户认证//
}
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
ROS_INFO("New User %d", nId);
if (g_bNeedPose)
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
else
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
void XN_CALLBACK_TYPE
Pose_Detected(xn::PoseDetectionCapability& pose, const XnChar* strPose,
XnUserID nId, void* pCookie) {
printf("Pose %s for user %d\n", strPose, nId);
FBLOG_INFO("Pose_Detected()", "Pose_Detected");
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
(*((hiKinect *)pCookie)->callbackPtr)->InvokeAsync("", FB::variant_list_of("POSE_DETECTED"));
}
static void XN_CALLBACK_TYPE poseDetectedCB(xn::PoseDetectionCapability &,
const XnChar* strPose,
XnUserID nId, void* /*pCookie*/)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
printf("%d Pose %s detected for user %d\n", epochTime, strPose, nId);
userGen.GetPoseDetectionCap().StopPoseDetection(nId);
userGen.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
void XN_CALLBACK_TYPE xn_onFoundUser
(xn::UserGenerator &user, XnUserID id, void *cookie)
{
fprintf(stderr, "found user %d\n", id);
if(gUserGenerator.GetSkeletonCap().NeedPoseForCalibration()){
gUserGenerator.GetPoseDetectionCap().StartPoseDetection(gPose, id);
}else{
gUserGenerator.GetSkeletonCap().RequestCalibration(id, true);
}
}
void XN_CALLBACK_TYPE UserFoundCB(
xn::UserGenerator& gen,
XnUserID id,
void* /*cookie*/
)
{
LOG(logDEBUG) << "UserFoundCB()";
gen.GetPoseDetectionCap().StartPoseDetection("Psi", id);
}
// Callback: New user was detected
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
printf("%d New User %d\n", epochTime, nId);
// New user found
if (g_bNeedPose) {
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
} else {
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, 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");
}
void XN_CALLBACK_TYPE CalibrationEnd(xn::SkeletonCapability& skeleton, XnUserID user, XnBool bSuccess, void* pCookie)
{
printf("Calibration complete for user %d: %s\n", user, bSuccess?"Success":"Failure");
if (bSuccess)
{
skeleton.StartTracking(user);
}
else
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection("Psi", user);
}
}
// Callback: Detected a pose
void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie)
{
printf("Pose %s detected for user %d\n", strPose, nId);
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
if(g_bhasCal){
g_UserGenerator.GetSkeletonCap().LoadCalibrationData(nId, 0);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else{ //never gotten calibration before
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
void XN_CALLBACK_TYPE xn_onCalibrated
(xn::SkeletonCapability &skel, XnUserID id, XnBool isSuccess, void *cookie)
{
if(isSuccess){
gUserGenerator.GetSkeletonCap().StartTracking(id);
}else{
if(gUserGenerator.GetSkeletonCap().NeedPoseForCalibration()){
gUserGenerator.GetPoseDetectionCap().StartPoseDetection(gPose, id);
}else{
gUserGenerator.GetSkeletonCap().RequestCalibration(id, true);
}
}
}
/**
* What to do when new user is found, ask if pose detection or calibration is
* required next.
*/
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
printf("\rNew User %d\n", nId);
// New userGenerator found
if (g_bNeedPose)
{
user.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
user.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
// ユーザー検出
void XN_CALLBACK_TYPE UserDetected(xn::UserGenerator& generator,
XnUserID nId, void* pCookie)
{
std::cout << "ユーザー検出:" << nId << " " << generator.GetNumberOfUsers() << "人目" << std::endl;
XnChar* pose = (XnChar*)pCookie;
if (pose[0] != '¥0') {
generator.GetPoseDetectionCap().StartPoseDetection(pose, nId);
}
else {
generator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) {
if (bSuccess) {
ROS_INFO("Calibration complete, start tracking user %d", nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else {
ROS_INFO("Calibration failed for user %d", nId);
if (g_bNeedPose)
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
else
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
void XN_CALLBACK_TYPE
Calibration_End(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
if (bSuccess)
{
printf("User calibrated\n");
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else
{
printf("Failed to calibrate user %d\n", nId);
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(POSE_TO_USE, nId);
}
}
static void XN_CALLBACK_TYPE newUserCB(xn::UserGenerator &, XnUserID nId,
void* /*pCookie*/)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
printf("%d New User %d\n", epochTime, nId);
// New User found
if (needPose){
userGen.GetPoseDetectionCap().StartPoseDetection(strPose, nId);
}
else{
userGen.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
// Callback: New user was detected
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& /*generator*/, XnUserID nId, void* /*pCookie*/){
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
printf("%d New User %d\n", epochTime, nId);
// New user found
if (g_bNeedPose)
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
else
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
// ICI ajout de l'utilisateur
Skeleton s(nId);
m_skeleton.insert(std::pair<XnUserID, Skeleton>(nId,s));
}
void XN_CALLBACK_TYPE UserCalibration_CalibrationComplete(xn::SkeletonCapability& capability, XnUserID nId, XnCalibrationStatus eStatus, void* pCookie)
{
if (eStatus == XN_CALIBRATION_STATUS_OK)
{
// Calibration succeeded
printf("Calibration complete, start tracking user %d\n", nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else
{
// Calibration failed
printf("Calibration failed for user %d\n", nId);
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection("Psi", nId);
}
}
void XN_CALLBACK_TYPE
User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
printf("New User: %d\n", nId);
FBLOG_INFO("User_NewUser", nId);
XnUInt16 nUsers = g_UserGenerator.GetNumberOfUsers();
XnUserID aUsers[nUsers];
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
return;
}
}
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(POSE_TO_USE, nId);
(*((hiKinect *)pCookie)->callbackPtr)->InvokeAsync("", FB::variant_list_of("NEW_USER"));
}
