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


skeleton_markers
Author(s): Patrick Goebel
autogenerated on Mon Oct 6 2014 07:41:01