miguelprada

Save/load skeleton calibration to a file

Aug 4th, 2011
184
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // openni_tracker.cpp
  2.  
  3. #include <ros/ros.h>
  4. #include <ros/package.h>
  5. #include <tf/transform_broadcaster.h>
  6. #include <kdl/frames.hpp>
  7.  
  8. #include <XnOpenNI.h>
  9. #include <XnCodecIDs.h>
  10. #include <XnCppWrapper.h>
  11.  
  12. #include <calibration_save_load/SaveCalibration.h>
  13. #include <calibration_save_load/LoadCalibration.h>
  14. #include <calibration_save_load/SaveCalibrationToFile.h>
  15. #include <calibration_save_load/LoadCalibrationFromFile.h>
  16.  
  17. using std::string;
  18.  
  19. xn::Context        g_Context;
  20. xn::DepthGenerator g_DepthGenerator;
  21. xn::UserGenerator  g_UserGenerator;
  22.  
  23. XnBool g_bNeedPose   = FALSE;
  24. XnChar g_strPose[20] = "";
  25.  
  26. void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
  27.     ROS_INFO("New User %d", nId);
  28.  
  29.     if (g_bNeedPose)
  30.         g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
  31.     else
  32.         g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
  33. }
  34.  
  35. void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
  36.     ROS_INFO("Lost user %d", nId);
  37. }
  38.  
  39. void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie) {
  40.     ROS_INFO("Calibration started for user %d", nId);
  41. }
  42.  
  43. void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) {
  44.     if (bSuccess) {
  45.         ROS_INFO("Calibration complete, start tracking user %d", nId);
  46.         g_UserGenerator.GetSkeletonCap().StartTracking(nId);
  47.     }
  48.     else {
  49.         ROS_INFO("Calibration failed for user %d", nId);
  50.         if (g_bNeedPose)
  51.             g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
  52.         else
  53.             g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
  54.     }
  55. }
  56.  
  57. void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, XnChar const* strPose, XnUserID nId, void* pCookie) {
  58.     ROS_INFO("Pose %s detected for user %d", strPose, nId);
  59.     g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
  60.     g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
  61. }
  62.  
  63. void publishTransform(XnUserID const& user, XnSkeletonJoint const& joint, string const& frame_id, string const& child_frame_id) {
  64.     static tf::TransformBroadcaster br;
  65.  
  66.     XnSkeletonJointPosition joint_position;
  67.     g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
  68.     double x = -joint_position.position.X / 1000.0;
  69.     double y = joint_position.position.Y / 1000.0;
  70.     double z = joint_position.position.Z / 1000.0;
  71.  
  72.     XnSkeletonJointOrientation joint_orientation;
  73.     g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
  74.  
  75.     XnFloat* m = joint_orientation.orientation.elements;
  76.     KDL::Rotation rotation(m[0], m[1], m[2],
  77.                            m[3], m[4], m[5],
  78.                            m[6], m[7], m[8]);
  79.     double qx, qy, qz, qw;
  80.     rotation.GetQuaternion(qx, qy, qz, qw);
  81.  
  82.     char child_frame_no[128];
  83.     snprintf(child_frame_no, sizeof(child_frame_no), "%s_%d", child_frame_id.c_str(), user);
  84.  
  85.     tf::Transform transform;
  86.     transform.setOrigin(tf::Vector3(x, y, z));
  87.     transform.setRotation(tf::Quaternion(qx, -qy, -qz, qw));
  88.  
  89.     // #4994
  90.     tf::Transform change_frame;
  91.     change_frame.setOrigin(tf::Vector3(0, 0, 0));
  92.     tf::Quaternion frame_rotation;
  93.     frame_rotation.setEulerZYX(1.5708, 0, 1.5708);
  94.     change_frame.setRotation(frame_rotation);
  95.  
  96.     transform = change_frame * transform;
  97.  
  98.     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_no));
  99. }
  100.  
  101. void publishTransforms(const std::string& frame_id) {
  102.     XnUserID users[15];
  103.     XnUInt16 users_count = 15;
  104.     g_UserGenerator.GetUsers(users, users_count);
  105.  
  106.     for (int i = 0; i < users_count; ++i) {
  107.         XnUserID user = users[i];
  108.         if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
  109.             continue;
  110.  
  111.  
  112.         publishTransform(user, XN_SKEL_HEAD,           frame_id, "head");
  113.         publishTransform(user, XN_SKEL_NECK,           frame_id, "neck");
  114.         publishTransform(user, XN_SKEL_TORSO,          frame_id, "torso");
  115.  
  116.         publishTransform(user, XN_SKEL_LEFT_SHOULDER,  frame_id, "left_shoulder");
  117.         publishTransform(user, XN_SKEL_LEFT_ELBOW,     frame_id, "left_elbow");
  118.         publishTransform(user, XN_SKEL_LEFT_HAND,      frame_id, "left_hand");
  119.  
  120.         publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
  121.         publishTransform(user, XN_SKEL_RIGHT_ELBOW,    frame_id, "right_elbow");
  122.         publishTransform(user, XN_SKEL_RIGHT_HAND,     frame_id, "right_hand");
  123.  
  124.         publishTransform(user, XN_SKEL_LEFT_HIP,       frame_id, "left_hip");
  125.         publishTransform(user, XN_SKEL_LEFT_KNEE,      frame_id, "left_knee");
  126.         publishTransform(user, XN_SKEL_LEFT_FOOT,      frame_id, "left_foot");
  127.  
  128.         publishTransform(user, XN_SKEL_RIGHT_HIP,      frame_id, "right_hip");
  129.         publishTransform(user, XN_SKEL_RIGHT_KNEE,     frame_id, "right_knee");
  130.         publishTransform(user, XN_SKEL_RIGHT_FOOT,     frame_id, "right_foot");
  131.     }
  132. }
  133.  
  134. bool saveCalibrationCallback(calibration_save_load::SaveCalibration::Request& req, calibration_save_load::SaveCalibration::Response& res)
  135. {
  136.     ROS_INFO_STREAM("Request to save calibration of user " << req.user << " into slot " << req.slot << " received");
  137.     if(g_UserGenerator.GetSkeletonCap().IsTracking(req.user))
  138.     {
  139.         ROS_INFO_STREAM("User skeleton is calibrated, trying to save the data");
  140.         XnStatus status = g_UserGenerator.GetSkeletonCap().SaveCalibrationData(req.user, req.slot);
  141.         if(status == XN_STATUS_OK)
  142.         {
  143.             ROS_INFO("Calibration data saved");
  144.             res.succeeded = true;
  145.         }
  146.         else
  147.         {
  148.             ROS_ERROR("Failed to save the calibration data");
  149.             xnPrintError(status, "ERROR");
  150.             res.error = "Failed to save skeleton calibration data";
  151.             res.succeeded = false;
  152.         }
  153.     }
  154.     else
  155.     {
  156.         ROS_ERROR("User is not being tracked");
  157.         res.error = "User is not being tracked";
  158.         res.succeeded = false;
  159.     }
  160.    
  161.     return true;
  162. }
  163.  
  164. bool loadCalibrationCallback(calibration_save_load::LoadCalibration::Request& req, calibration_save_load::LoadCalibration::Response& res)
  165. {
  166.     XnUserID users[15];
  167.     XnUInt16 users_count = 15;
  168.     g_UserGenerator.GetUsers(users, users_count);
  169.     bool detected = false;
  170.    
  171.     for(int i=0; i<users_count; ++i)
  172.     {
  173.         if(users[i] == XnUserID(req.user))
  174.         {
  175.             detected = true;
  176.             break;
  177.         }
  178.     }
  179.    
  180.     if(!detected)
  181.     {
  182.         res.error = "No user with specified id has been detected";
  183.         res.succeeded = false;
  184.         return true;
  185.     }
  186.    
  187.     if(g_UserGenerator.GetSkeletonCap().LoadCalibrationData(req.user, req.slot) == XN_STATUS_OK)
  188.     {
  189.         g_UserGenerator.GetSkeletonCap().StartTracking(req.user);
  190.         res.succeeded = true;
  191.     }
  192.     else
  193.     {
  194.         res.error = "Failed to load skeleton calibration data";
  195.         res.succeeded = false;
  196.     }
  197.    
  198.     return true;
  199. }
  200.  
  201. bool saveCalibrationToFileCallback(calibration_save_load::SaveCalibrationToFile::Request& req, calibration_save_load::SaveCalibrationToFile::Response& res)
  202. {
  203.     ROS_INFO_STREAM("Request to save calibration of user " << req.user << " into file " << req.filename << " received");
  204.     if(g_UserGenerator.GetSkeletonCap().IsTracking(req.user))
  205.     {
  206.         ROS_INFO_STREAM("User skeleton is calibrated, trying to save the data");
  207.         XnStatus status = g_UserGenerator.GetSkeletonCap().SaveCalibrationDataToFile(req.user, req.filename.c_str());
  208.         if(status == XN_STATUS_OK)
  209.         {
  210.             ROS_INFO("Calibration data saved");
  211.             res.succeeded = true;
  212.         }
  213.         else
  214.         {
  215.             ROS_ERROR("Failed to save the calibration data");
  216.             xnPrintError(status, "ERROR");
  217.             res.error = "Failed to save skeleton calibration data";
  218.             res.succeeded = false;
  219.         }
  220.     }
  221.     else
  222.     {
  223.         ROS_ERROR("User is not being tracked");
  224.         res.error = "User is not being tracked";
  225.         res.succeeded = false;
  226.     }
  227.    
  228.     return true;
  229. }
  230.  
  231. bool loadCalibrationFromFileCallback(calibration_save_load::LoadCalibrationFromFile::Request& req, calibration_save_load::LoadCalibrationFromFile::Response& res)
  232. {
  233.     XnUserID users[15];
  234.     XnUInt16 users_count = 15;
  235.     g_UserGenerator.GetUsers(users, users_count);
  236.     bool detected = false;
  237.    
  238.     for(int i=0; i<users_count; ++i)
  239.     {
  240.         if(users[i] == XnUserID(req.user))
  241.         {
  242.             detected = true;
  243.             break;
  244.         }
  245.     }
  246.    
  247.     if(!detected)
  248.     {
  249.         res.error = "No user with specified id has been detected";
  250.         res.succeeded = false;
  251.         return true;
  252.     }
  253.  
  254.     XnStatus status = g_UserGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(req.user, req.filename.c_str());
  255.     if(status == XN_STATUS_OK)
  256.     {
  257.         res.succeeded = true;
  258.     }
  259.     else
  260.     {
  261.         ROS_ERROR("Failed to save the calibration data");
  262.         xnPrintError(status, "ERROR");
  263.         res.error = "Failed to load skeleton calibration data";
  264.         res.succeeded = false;
  265.     }
  266.    
  267.     return true;
  268. }
  269.  
  270. #define CHECK_RC(nRetVal, what)                                     \
  271.     if (nRetVal != XN_STATUS_OK)                                    \
  272.     {                                                               \
  273.         ROS_ERROR("%s failed: %s", what, xnGetStatusString(nRetVal));\
  274.         return nRetVal;                                             \
  275.     }
  276.  
  277. int main(int argc, char **argv) {
  278.     ros::init(argc, argv, "openni_tracker");
  279.     ros::NodeHandle nh;
  280.  
  281.     string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
  282.     XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
  283.     CHECK_RC(nRetVal, "InitFromXml");
  284.  
  285.     nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
  286.     CHECK_RC(nRetVal, "Find depth generator");
  287.  
  288.     nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
  289.     if (nRetVal != XN_STATUS_OK) {
  290.         nRetVal = g_UserGenerator.Create(g_Context);
  291.         CHECK_RC(nRetVal, "Find user generator");
  292.     }
  293.  
  294.     if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
  295.         ROS_INFO("Supplied user generator doesn't support skeleton");
  296.         return 1;
  297.     }
  298.  
  299.     XnCallbackHandle hUserCallbacks;
  300.     g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
  301.  
  302.     XnCallbackHandle hCalibrationCallbacks;
  303.     g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
  304.  
  305.     if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) {
  306.         g_bNeedPose = TRUE;
  307.         if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
  308.             ROS_INFO("Pose required, but not supported");
  309.             return 1;
  310.         }
  311.  
  312.         XnCallbackHandle hPoseCallbacks;
  313.         g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
  314.  
  315.         g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
  316.     }
  317.  
  318.     g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
  319.  
  320.     nRetVal = g_Context.StartGeneratingAll();
  321.     CHECK_RC(nRetVal, "StartGenerating");
  322.  
  323.     ros::Rate r(30);
  324.  
  325.     ros::NodeHandle pnh("~");
  326.     string frame_id("openni_depth_frame");
  327.     pnh.getParam("camera_frame_id", frame_id);
  328.  
  329.     ros::ServiceServer save_calibration_service = pnh.advertiseService("save_skeleton_calibration", saveCalibrationCallback);
  330.     ros::ServiceServer load_calibration_service = pnh.advertiseService("load_skeleton_calibration", loadCalibrationCallback);
  331.     ros::ServiceServer save_calibration_to_file_service = pnh.advertiseService("save_skeleton_calibration_to_file", saveCalibrationToFileCallback);
  332.     ros::ServiceServer load_calibration_from_file_service = pnh.advertiseService("load_skeleton_calibration_from_file", loadCalibrationFromFileCallback);
  333.    
  334.     while (ros::ok()) {
  335.         g_Context.WaitAndUpdateAll();
  336.         publishTransforms(frame_id);
  337.         ros::spinOnce();
  338.         r.sleep();
  339.     }
  340.  
  341.     g_Context.Shutdown();
  342.     return 0;
  343. }
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×