00001
00002
00003
00004
00005
00006 #include <ros/ros.h>
00007 #include <std_msgs/String.h>
00008
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;
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
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