bool DataCapture::stopDataCapture()
{
context_.StopGeneratingAll();
context_.Shutdown(); // do once. should probably be in uninitialise()
return true;
}
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;
}
void Context_StopGeneratingAll_wrapped(xn::Context& self) {
check( self.StopGeneratingAll() );
}
int main() {
printf("main() START\n");
signal(SIGINT, onSignalReceived); // hit CTRL-C keys in terminal (2)
signal(SIGTERM, onSignalReceived); // hit stop button in eclipse CDT (15)
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS = 30;
CHECK_RC(ctx.Init(), "init");
CHECK_RC(depthGenerator.Create(ctx), "create depth");
depthGenerator.SetMapOutputMode(mapMode);
XnStatus userAvailable = ctx.FindExistingNode(XN_NODE_TYPE_USER, userGenerator);
if(userAvailable != XN_STATUS_OK) {
CHECK_RC(userGenerator.Create(ctx), "create user");
}
XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
xn::SkeletonCapability skel = userGenerator.GetSkeletonCap();
CHECK_RC(userGenerator.RegisterUserCallbacks(onUserNew, onUserLost, NULL, hUserCallbacks), "register user");
CHECK_RC(skel.RegisterCalibrationCallbacks(onCalibrationStart, onCalibrationEnd, NULL, hCalibrationCallbacks), "register calib");
CHECK_RC(userGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(onPoseDetected, NULL, NULL, hPoseCallbacks), "register pose");
XnChar poseName[20] = "";
CHECK_RC(skel.GetCalibrationPose(poseName), "get posename");
printf("poseName: %s\n", poseName);
CHECK_RC(skel.SetSkeletonProfile(XN_SKEL_PROFILE_ALL), "set skel profile");
CHECK_RC(skel.SetSmoothing(0.8), "set smoothing");
// xnSetMirror(depth, !mirrorMode);
CHECK_RC(ctx.StartGeneratingAll(), "start generating");
printf("waitAnyUpdateAll ...\n");
while(!shouldTerminate) {
ctx.WaitAnyUpdateAll();
// depthGenerator.GetMetaData(tempDepthMetaData);
const XnUInt16 userCount = userGenerator.GetNumberOfUsers();
// printf("userCount: %i\n", userCount);
XnUserID aUsers[userCount];
XnUInt16 nUsers = userCount;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
XnUserID currentUserId = aUsers[i];
if (skel.IsTracking(aUsers[i])) {
XnSkeletonJointPosition joint;
skel.GetSkeletonJointPosition(currentUserId, XN_SKEL_HEAD, joint);
XnFloat x = joint.position.X;
XnFloat y = joint.position.Y;
XnFloat z = joint.position.Z;
printf("joint position: %.2f x %.2f x %.2f\n", x, y, z);
printf("joint.fConfidence: %.2f\n", joint.fConfidence);
}
}
}
printf("STOP\n");
CHECK_RC(ctx.StopGeneratingAll(), "stop generating");
ctx.Shutdown();
printf("main() END\n");
}