openni_tracker.cpp
Go to the documentation of this file.
00001 // openni_tracker.cpp
00002 
00003 #include <ros/ros.h>
00004 #include <ros/package.h>
00005 #include <tf/transform_broadcaster.h>
00006 #include <kdl/frames.hpp>
00007 
00008 #include <XnOpenNI.h>
00009 #include <XnCodecIDs.h>
00010 #include <XnCppWrapper.h>
00011 
00012 using std::string;
00013 
00014 xn::Context        g_Context;
00015 xn::DepthGenerator g_DepthGenerator;
00016 xn::UserGenerator  g_UserGenerator;
00017 
00018 XnBool g_bNeedPose   = FALSE;
00019 XnChar g_strPose[20] = "";
00020 
00021 void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
00022         ROS_INFO("New User %d", nId);
00023 
00024         if (g_bNeedPose)
00025                 g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
00026         else
00027                 g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
00028 }
00029 
00030 void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
00031         ROS_INFO("Lost user %d", nId);
00032 }
00033 
00034 void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie) {
00035         ROS_INFO("Calibration started for user %d", nId);
00036 }
00037 
00038 void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) {
00039         if (bSuccess) {
00040                 ROS_INFO("Calibration complete, start tracking user %d", nId);
00041                 g_UserGenerator.GetSkeletonCap().StartTracking(nId);
00042         }
00043         else {
00044                 ROS_INFO("Calibration failed for user %d", nId);
00045                 if (g_bNeedPose)
00046                         g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
00047                 else
00048                         g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
00049         }
00050 }
00051 
00052 void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, XnChar const* strPose, XnUserID nId, void* pCookie) {
00053     ROS_INFO("Pose %s detected for user %d", strPose, nId);
00054     g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
00055     g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
00056 }
00057 
00058 void publishTransform(XnUserID const& user, XnSkeletonJoint const& joint, string const& frame_id, string const& child_frame_id) {
00059     static tf::TransformBroadcaster br;
00060 
00061     XnSkeletonJointPosition joint_position;
00062     g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
00063     double x = -joint_position.position.X / 1000.0;
00064     double y = joint_position.position.Y / 1000.0;
00065     double z = joint_position.position.Z / 1000.0;
00066 
00067     XnSkeletonJointOrientation joint_orientation;
00068     g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
00069 
00070     XnFloat* m = joint_orientation.orientation.elements;
00071     KDL::Rotation rotation(m[0], m[1], m[2],
00072                                            m[3], m[4], m[5],
00073                                            m[6], m[7], m[8]);
00074     double qx, qy, qz, qw;
00075     rotation.GetQuaternion(qx, qy, qz, qw);
00076 
00077     char child_frame_no[128];
00078     snprintf(child_frame_no, sizeof(child_frame_no), "%s_%d", child_frame_id.c_str(), user);
00079 
00080     tf::Transform transform;
00081     transform.setOrigin(tf::Vector3(x, y, z));
00082     transform.setRotation(tf::Quaternion(qx, -qy, -qz, qw));
00083 
00084     // #4994
00085     tf::Transform change_frame;
00086     change_frame.setOrigin(tf::Vector3(0, 0, 0));
00087     tf::Quaternion frame_rotation;
00088     frame_rotation.setEulerZYX(1.5708, 0, 1.5708);
00089     change_frame.setRotation(frame_rotation);
00090 
00091     transform = change_frame * transform;
00092 
00093     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_no));
00094 }
00095 
00096 void publishTransforms(const std::string& frame_id) {
00097     XnUserID users[15];
00098     XnUInt16 users_count = 15;
00099     g_UserGenerator.GetUsers(users, users_count);
00100 
00101     for (int i = 0; i < users_count; ++i) {
00102         XnUserID user = users[i];
00103         if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
00104             continue;
00105 
00106 
00107         publishTransform(user, XN_SKEL_HEAD,           frame_id, "head");
00108         publishTransform(user, XN_SKEL_NECK,           frame_id, "neck");
00109         publishTransform(user, XN_SKEL_TORSO,          frame_id, "torso");
00110 
00111         publishTransform(user, XN_SKEL_LEFT_SHOULDER,  frame_id, "left_shoulder");
00112         publishTransform(user, XN_SKEL_LEFT_ELBOW,     frame_id, "left_elbow");
00113         publishTransform(user, XN_SKEL_LEFT_HAND,      frame_id, "left_hand");
00114 
00115         publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
00116         publishTransform(user, XN_SKEL_RIGHT_ELBOW,    frame_id, "right_elbow");
00117         publishTransform(user, XN_SKEL_RIGHT_HAND,     frame_id, "right_hand");
00118 
00119         publishTransform(user, XN_SKEL_LEFT_HIP,       frame_id, "left_hip");
00120         publishTransform(user, XN_SKEL_LEFT_KNEE,      frame_id, "left_knee");
00121         publishTransform(user, XN_SKEL_LEFT_FOOT,      frame_id, "left_foot");
00122 
00123         publishTransform(user, XN_SKEL_RIGHT_HIP,      frame_id, "right_hip");
00124         publishTransform(user, XN_SKEL_RIGHT_KNEE,     frame_id, "right_knee");
00125         publishTransform(user, XN_SKEL_RIGHT_FOOT,     frame_id, "right_foot");
00126     }
00127 }
00128 
00129 #define CHECK_RC(nRetVal, what)                                                                         \
00130         if (nRetVal != XN_STATUS_OK)                                                                    \
00131         {                                                                                                                               \
00132                 ROS_ERROR("%s failed: %s", what, xnGetStatusString(nRetVal));\
00133                 return nRetVal;                                                                                         \
00134         }
00135 
00136 int main(int argc, char **argv) {
00137     ros::init(argc, argv, "openni_tracker");
00138     ros::NodeHandle nh;
00139 
00140     string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
00141     XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
00142     CHECK_RC(nRetVal, "InitFromXml");
00143 
00144     nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
00145     CHECK_RC(nRetVal, "Find depth generator");
00146 
00147         nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
00148         if (nRetVal != XN_STATUS_OK) {
00149                 nRetVal = g_UserGenerator.Create(g_Context);
00150             if (nRetVal != XN_STATUS_OK) {
00151                     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));
00152             return nRetVal;
00153             }
00154         }
00155 
00156         if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
00157                 ROS_INFO("Supplied user generator doesn't support skeleton");
00158                 return 1;
00159         }
00160 
00161     XnCallbackHandle hUserCallbacks;
00162         g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
00163 
00164         XnCallbackHandle hCalibrationCallbacks;
00165         g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
00166 
00167         if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) {
00168                 g_bNeedPose = TRUE;
00169                 if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
00170                         ROS_INFO("Pose required, but not supported");
00171                         return 1;
00172                 }
00173 
00174                 XnCallbackHandle hPoseCallbacks;
00175                 g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
00176 
00177                 g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
00178         }
00179 
00180         g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
00181 
00182         nRetVal = g_Context.StartGeneratingAll();
00183         CHECK_RC(nRetVal, "StartGenerating");
00184 
00185         ros::Rate r(30);
00186 
00187         
00188         ros::NodeHandle pnh("~");
00189         string frame_id("openni_depth_frame");
00190         pnh.getParam("camera_frame_id", frame_id);
00191                 
00192         while (ros::ok()) {
00193                 g_Context.WaitAndUpdateAll();
00194                 publishTransforms(frame_id);
00195                 r.sleep();
00196         }
00197 
00198         g_Context.Shutdown();
00199         return 0;
00200 }


openni_tracker
Author(s): Tim Field
autogenerated on Wed Aug 26 2015 15:10:10