Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // openni_tracker.cpp
- #include <ros/ros.h>
- #include <ros/package.h>
- #include <tf/transform_broadcaster.h>
- #include <kdl/frames.hpp>
- #include <XnOpenNI.h>
- #include <XnCodecIDs.h>
- #include <XnCppWrapper.h>
- #include <calibration_save_load/SaveCalibration.h>
- #include <calibration_save_load/LoadCalibration.h>
- #include <calibration_save_load/SaveCalibrationToFile.h>
- #include <calibration_save_load/LoadCalibrationFromFile.h>
- using std::string;
- xn::Context g_Context;
- xn::DepthGenerator g_DepthGenerator;
- xn::UserGenerator g_UserGenerator;
- XnBool g_bNeedPose = FALSE;
- XnChar g_strPose[20] = "";
- 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 User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
- ROS_INFO("Lost user %d", nId);
- }
- void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie) {
- ROS_INFO("Calibration started for user %d", nId);
- }
- 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 UserPose_PoseDetected(xn::PoseDetectionCapability& capability, XnChar const* strPose, XnUserID nId, void* pCookie) {
- ROS_INFO("Pose %s detected for user %d", strPose, nId);
- g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
- g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
- }
- void publishTransform(XnUserID const& user, XnSkeletonJoint const& joint, string const& frame_id, string const& child_frame_id) {
- static tf::TransformBroadcaster br;
- XnSkeletonJointPosition joint_position;
- g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
- double x = -joint_position.position.X / 1000.0;
- double y = joint_position.position.Y / 1000.0;
- double z = joint_position.position.Z / 1000.0;
- XnSkeletonJointOrientation joint_orientation;
- g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
- XnFloat* m = joint_orientation.orientation.elements;
- KDL::Rotation rotation(m[0], m[1], m[2],
- m[3], m[4], m[5],
- m[6], m[7], m[8]);
- double qx, qy, qz, qw;
- rotation.GetQuaternion(qx, qy, qz, qw);
- char child_frame_no[128];
- snprintf(child_frame_no, sizeof(child_frame_no), "%s_%d", child_frame_id.c_str(), user);
- tf::Transform transform;
- transform.setOrigin(tf::Vector3(x, y, z));
- transform.setRotation(tf::Quaternion(qx, -qy, -qz, qw));
- // #4994
- tf::Transform change_frame;
- change_frame.setOrigin(tf::Vector3(0, 0, 0));
- tf::Quaternion frame_rotation;
- frame_rotation.setEulerZYX(1.5708, 0, 1.5708);
- change_frame.setRotation(frame_rotation);
- transform = change_frame * transform;
- br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_no));
- }
- void publishTransforms(const std::string& frame_id) {
- XnUserID users[15];
- XnUInt16 users_count = 15;
- g_UserGenerator.GetUsers(users, users_count);
- for (int i = 0; i < users_count; ++i) {
- XnUserID user = users[i];
- if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
- continue;
- publishTransform(user, XN_SKEL_HEAD, frame_id, "head");
- publishTransform(user, XN_SKEL_NECK, frame_id, "neck");
- publishTransform(user, XN_SKEL_TORSO, frame_id, "torso");
- publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder");
- publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow");
- publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand");
- publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
- publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow");
- publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand");
- publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip");
- publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee");
- publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot");
- publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip");
- publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee");
- publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot");
- }
- }
- bool saveCalibrationCallback(calibration_save_load::SaveCalibration::Request& req, calibration_save_load::SaveCalibration::Response& res)
- {
- ROS_INFO_STREAM("Request to save calibration of user " << req.user << " into slot " << req.slot << " received");
- if(g_UserGenerator.GetSkeletonCap().IsTracking(req.user))
- {
- ROS_INFO_STREAM("User skeleton is calibrated, trying to save the data");
- XnStatus status = g_UserGenerator.GetSkeletonCap().SaveCalibrationData(req.user, req.slot);
- if(status == XN_STATUS_OK)
- {
- ROS_INFO("Calibration data saved");
- res.succeeded = true;
- }
- else
- {
- ROS_ERROR("Failed to save the calibration data");
- xnPrintError(status, "ERROR");
- res.error = "Failed to save skeleton calibration data";
- res.succeeded = false;
- }
- }
- else
- {
- ROS_ERROR("User is not being tracked");
- res.error = "User is not being tracked";
- res.succeeded = false;
- }
- return true;
- }
- bool loadCalibrationCallback(calibration_save_load::LoadCalibration::Request& req, calibration_save_load::LoadCalibration::Response& res)
- {
- XnUserID users[15];
- XnUInt16 users_count = 15;
- g_UserGenerator.GetUsers(users, users_count);
- bool detected = false;
- for(int i=0; i<users_count; ++i)
- {
- if(users[i] == XnUserID(req.user))
- {
- detected = true;
- break;
- }
- }
- if(!detected)
- {
- res.error = "No user with specified id has been detected";
- res.succeeded = false;
- return true;
- }
- if(g_UserGenerator.GetSkeletonCap().LoadCalibrationData(req.user, req.slot) == XN_STATUS_OK)
- {
- g_UserGenerator.GetSkeletonCap().StartTracking(req.user);
- res.succeeded = true;
- }
- else
- {
- res.error = "Failed to load skeleton calibration data";
- res.succeeded = false;
- }
- return true;
- }
- bool saveCalibrationToFileCallback(calibration_save_load::SaveCalibrationToFile::Request& req, calibration_save_load::SaveCalibrationToFile::Response& res)
- {
- ROS_INFO_STREAM("Request to save calibration of user " << req.user << " into file " << req.filename << " received");
- if(g_UserGenerator.GetSkeletonCap().IsTracking(req.user))
- {
- ROS_INFO_STREAM("User skeleton is calibrated, trying to save the data");
- XnStatus status = g_UserGenerator.GetSkeletonCap().SaveCalibrationDataToFile(req.user, req.filename.c_str());
- if(status == XN_STATUS_OK)
- {
- ROS_INFO("Calibration data saved");
- res.succeeded = true;
- }
- else
- {
- ROS_ERROR("Failed to save the calibration data");
- xnPrintError(status, "ERROR");
- res.error = "Failed to save skeleton calibration data";
- res.succeeded = false;
- }
- }
- else
- {
- ROS_ERROR("User is not being tracked");
- res.error = "User is not being tracked";
- res.succeeded = false;
- }
- return true;
- }
- bool loadCalibrationFromFileCallback(calibration_save_load::LoadCalibrationFromFile::Request& req, calibration_save_load::LoadCalibrationFromFile::Response& res)
- {
- XnUserID users[15];
- XnUInt16 users_count = 15;
- g_UserGenerator.GetUsers(users, users_count);
- bool detected = false;
- for(int i=0; i<users_count; ++i)
- {
- if(users[i] == XnUserID(req.user))
- {
- detected = true;
- break;
- }
- }
- if(!detected)
- {
- res.error = "No user with specified id has been detected";
- res.succeeded = false;
- return true;
- }
- XnStatus status = g_UserGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(req.user, req.filename.c_str());
- if(status == XN_STATUS_OK)
- {
- res.succeeded = true;
- }
- else
- {
- ROS_ERROR("Failed to save the calibration data");
- xnPrintError(status, "ERROR");
- res.error = "Failed to load skeleton calibration data";
- res.succeeded = false;
- }
- return true;
- }
- #define CHECK_RC(nRetVal, what) \
- if (nRetVal != XN_STATUS_OK) \
- { \
- ROS_ERROR("%s failed: %s", what, xnGetStatusString(nRetVal));\
- return nRetVal; \
- }
- 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)) {
- 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::ServiceServer save_calibration_service = pnh.advertiseService("save_skeleton_calibration", saveCalibrationCallback);
- ros::ServiceServer load_calibration_service = pnh.advertiseService("load_skeleton_calibration", loadCalibrationCallback);
- ros::ServiceServer save_calibration_to_file_service = pnh.advertiseService("save_skeleton_calibration_to_file", saveCalibrationToFileCallback);
- ros::ServiceServer load_calibration_from_file_service = pnh.advertiseService("load_skeleton_calibration_from_file", loadCalibrationFromFileCallback);
- while (ros::ok()) {
- g_Context.WaitAndUpdateAll();
- publishTransforms(frame_id);
- ros::spinOnce();
- r.sleep();
- }
- g_Context.Shutdown();
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement