skeleton_tracker.cpp
Go to the documentation of this file.
00001 // This code is a modification of Taylor Veltrop's kinect_teleop.cpp
00002 // file found in his Veltrobot package at:
00003 //
00004 // http://www.ros.org/wiki/veltrop-ros-pkg
00005 
00006 #include <ros/ros.h>
00007 #include <std_msgs/String.h>
00008 //#include <roslib/Header.h>
00009 #include <std_msgs/Header.h>
00010 #include <pi_tracker/Skeleton.h>
00011 #include <tf/transform_broadcaster.h>
00012 #include <kdl/frames.hpp>
00013 #include <GL/glut.h>
00014 #include <string>
00015 #include "KinectController.h"
00016 #include "KinectDisplay.h"
00017 
00018 using std::string;
00019 
00020 #ifndef PI
00021 #define PI 3.14159265359
00022 #endif
00023 #ifndef HALFPI
00024 #define HALFPI 1.57079632679
00025 #endif
00026 #ifndef QUARTPI
00027 #define QUARTPI 0.785398163397
00028 #endif
00029 
00030 namespace skeleton_tracker
00031 {
00032         class SkeletonTracker
00033   {
00034     public:
00035       std::string fixed_frame;
00036 
00037       SkeletonTracker()
00038       {    
00039       }
00040       
00041       void init()
00042       {
00043         ros::NodeHandle n;
00044             int rate;
00045             n.param("tracking_rate", rate, 1);
00046             n.param("fixed_frame", fixed_frame, std::string("openni_depth_frame"));
00047         skeleton_pub_ = n.advertise<pi_tracker::Skeleton>("/skeleton", rate);
00048       }
00049       
00050       void publishTransform(KinectController &kinect_controller, XnUserID const &user, XnSkeletonJoint const &joint, string const &frame_id, string const &child_frame_id, pi_tracker::Skeleton &skeleton)
00051       {
00052 
00053         xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
00054         static tf::TransformBroadcaster br;
00055 
00056         XnSkeletonJointPosition joint_position;
00057         UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
00058         double x = joint_position.position.X / 1000.0;
00059         double y = joint_position.position.Y / 1000.0;
00060         double z = joint_position.position.Z / 1000.0;
00061 
00062         XnSkeletonJointOrientation joint_orientation;
00063         UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
00064 
00065         XnFloat* m = joint_orientation.orientation.elements;
00066         KDL::Rotation rotation(m[0], m[1], m[2],
00067                                m[3], m[4], m[5],
00068                                m[6], m[7], m[8]);
00069         double qx, qy, qz, qw;
00070         rotation.GetQuaternion(qx, qy, qz, qw);
00071 
00072                 geometry_msgs::Vector3 position;
00073                 geometry_msgs::Quaternion orientation;
00074 
00075                 position.x = x;
00076                 position.y = y;
00077                 position.z = z;
00078 
00079                 orientation.x = qx;
00080                 orientation.y = qy;
00081                 orientation.z = qz;
00082                 orientation.w = qw;
00083 
00084                 skeleton.name.push_back(child_frame_id);
00085                 skeleton.position.push_back(position);
00086                 skeleton.orientation.push_back(orientation);
00087                 skeleton.confidence.push_back(joint_position.fConfidence);
00088 
00089         tf::Transform transform;
00090         transform.setOrigin(tf::Vector3(x, y, z));
00091         transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
00092         br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_id));
00093       }
00094         
00095     void processKinect(KinectController &kinect_controller)
00096       {
00097         XnUserID users[15];
00098         XnUInt16 users_count = 15;
00099         xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
00100         UserGenerator.GetUsers(users, users_count);
00101             pi_tracker::Skeleton g_skel;
00102 
00103         for (int i = 0; i < users_count; ++i)
00104         {
00105           XnUserID user = users[i];
00106           if (!UserGenerator.GetSkeletonCap().IsTracking(user))
00107             continue;
00108             
00109           publishTransform(kinect_controller, user, XN_SKEL_HEAD,           fixed_frame, "head", g_skel);
00110           publishTransform(kinect_controller, user, XN_SKEL_NECK,           fixed_frame, "neck", g_skel);
00111           publishTransform(kinect_controller, user, XN_SKEL_TORSO,          fixed_frame, "torso", g_skel);
00112 
00113           publishTransform(kinect_controller, user, XN_SKEL_LEFT_SHOULDER,  fixed_frame, "left_shoulder", g_skel);
00114           publishTransform(kinect_controller, user, XN_SKEL_LEFT_ELBOW,     fixed_frame, "left_elbow", g_skel);
00115           publishTransform(kinect_controller, user, XN_SKEL_LEFT_HAND,      fixed_frame, "left_hand", g_skel);
00116 
00117           publishTransform(kinect_controller, user, XN_SKEL_RIGHT_SHOULDER, fixed_frame, "right_shoulder", g_skel);
00118           publishTransform(kinect_controller, user, XN_SKEL_RIGHT_ELBOW,    fixed_frame, "right_elbow", g_skel);
00119           publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HAND,     fixed_frame, "right_hand", g_skel);
00120 
00121           publishTransform(kinect_controller, user, XN_SKEL_LEFT_HIP,       fixed_frame, "left_hip", g_skel);
00122           publishTransform(kinect_controller, user, XN_SKEL_LEFT_KNEE,      fixed_frame, "left_knee", g_skel);
00123           publishTransform(kinect_controller, user, XN_SKEL_LEFT_FOOT,      fixed_frame, "left_foot", g_skel);
00124 
00125           publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HIP,      fixed_frame, "right_hip", g_skel);
00126           publishTransform(kinect_controller, user, XN_SKEL_RIGHT_KNEE,     fixed_frame, "right_knee", g_skel);
00127           publishTransform(kinect_controller, user, XN_SKEL_RIGHT_FOOT,     fixed_frame, "right_foot", g_skel);
00128 
00129               g_skel.user_id = user;
00130               g_skel.header.stamp = ros::Time::now();
00131               g_skel.header.frame_id = fixed_frame;
00132           skeleton_pub_.publish(g_skel);
00133               break;    // only read first user
00134         }
00135       }
00136          
00137   private:
00138     ros::Publisher  skeleton_pub_;    
00139   };
00140 
00141 }
00142 
00143 
00144 #define GL_WIN_SIZE_X 720
00145 #define GL_WIN_SIZE_Y 480
00146 KinectController g_kinect_controller;
00147 skeleton_tracker::SkeletonTracker g_skeleton_tracker;
00148 
00149 void glutIdle (void)
00150 {
00151         glutPostRedisplay();
00152 }
00153 
00154 void glutDisplay (void)
00155 {
00156         xn::SceneMetaData sceneMD;
00157         xn::DepthMetaData depthMD;
00158   
00159   g_kinect_controller.getContext().WaitAndUpdateAll();
00160   g_skeleton_tracker.processKinect(g_kinect_controller);
00161   g_kinect_controller.getDepthGenerator().GetMetaData(depthMD);
00162   g_kinect_controller.getUserGenerator().GetUserPixels(0, sceneMD);  
00163         
00164         glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00165 
00166         // Setup the OpenGL viewpoint
00167         glMatrixMode(GL_PROJECTION);
00168         glPushMatrix();
00169         glLoadIdentity();
00170 
00171         glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
00172   
00173   glDisable(GL_TEXTURE_2D);
00174   
00175   kinect_display_drawDepthMapGL(depthMD, sceneMD);
00176         kinect_display_drawSkeletonGL(g_kinect_controller.getUserGenerator(),
00177                                 g_kinect_controller.getDepthGenerator());  
00178   
00179         glutSwapBuffers();
00180 }
00181 
00182 void glutKeyboard (unsigned char key, int x, int y)
00183 {
00184         switch (key)
00185         {
00186         case 27:
00187     exit(1);
00188     break;
00189         }
00190 }
00191 
00192 int main(int argc, char** argv)
00193 {
00194   ros::init(argc, argv, "skeleton_tracker");
00195   ros::NodeHandle np("~");
00196 
00197   string filepath;
00198   bool is_a_recording;
00199   np.getParam("load_filepath", filepath); 
00200   np.param<bool>("load_recording", is_a_recording, false);      
00201 
00202   g_skeleton_tracker.init();
00203   g_kinect_controller.init(filepath.c_str(), is_a_recording);
00204   
00205   glutInit(&argc, argv);
00206   glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
00207   glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
00208   glutCreateWindow ("NITE Skeleton Tracker");
00209 
00210   glutKeyboardFunc(glutKeyboard);
00211   glutDisplayFunc(glutDisplay);
00212   glutIdleFunc(glutIdle);
00213 
00214   glDisable(GL_DEPTH_TEST);
00215   glEnable(GL_TEXTURE_2D);
00216 
00217   glEnableClientState(GL_VERTEX_ARRAY);
00218   glDisableClientState(GL_COLOR_ARRAY);  
00219   
00220   glutMainLoop();
00221   
00222   g_kinect_controller.shutdown();
00223   
00224   return 0;
00225 }
00226 
00227 


pi_tracker
Author(s): Patrick Goebel
autogenerated on Tue Jan 7 2014 11:27:49