Fawkes API  Fawkes Development Version
main.cpp
00001 
00002 /***************************************************************************
00003  *  main.cpp - Skeleton Visualization GUI: main program
00004  *
00005  *  Created: Wed Mar 02 11:15:53 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "skel_drawer.h"
00024 #include "image_drawer.h"
00025 #include "depth_drawer.h"
00026 #include <plugins/openni/utils/skel_if_observer.h>
00027 #include <plugins/openni/utils/hand_if_observer.h>
00028 
00029 #include <core/threading/thread.h>
00030 #include <core/threading/mutex.h>
00031 #include <blackboard/remote.h>
00032 #include <blackboard/interface_observer.h>
00033 #include <utils/system/argparser.h>
00034 #include <fvcams/net.h>
00035 #include <fvcams/shmem.h>
00036 
00037 #include <map>
00038 #include <string>
00039 #include <queue>
00040 #include <cstdio>
00041 #include <cstring>
00042 #include <GL/glut.h>
00043 
00044 #define GL_WIN_SIZE_X 640
00045 #define GL_WIN_SIZE_Y 480
00046 
00047 using namespace fawkes;
00048 using namespace fawkes::openni;
00049 using namespace firevision;
00050 
00051 bool g_quit = false;
00052 bool g_pause = false;
00053 bool g_draw_image = true;
00054 bool g_draw_depth = false;
00055 bool g_draw_skeleton = true;
00056 RemoteBlackBoard *g_rbb;
00057 Camera *g_image_cam;
00058 Camera *g_depth_cam;
00059 Camera *g_label_cam;
00060 SkelGuiImageDrawer *g_image_drawer = NULL;
00061 SkelGuiDepthDrawer *g_depth_drawer = NULL;
00062 SkelGuiSkeletonDrawer *g_skeleton_drawer = NULL;
00063 
00064 UserMap  g_users;
00065 HandMap  g_hands;
00066 
00067 SkelIfObserver *g_obs;
00068 HandIfObserver *g_hands_obs;
00069 
00070 void clean_exit()
00071 {
00072   delete g_obs;
00073   delete g_hands_obs;
00074   for (UserMap::iterator i = g_users.begin(); i != g_users.end(); ++i) {
00075     g_rbb->close(i->second.skel_if);
00076     g_rbb->close(i->second.proj_if);
00077   }
00078   delete g_rbb;
00079 
00080   delete g_image_cam;
00081   delete g_depth_cam;
00082   delete g_label_cam;
00083   delete g_image_drawer;
00084   delete g_depth_drawer;
00085   delete g_skeleton_drawer;
00086 
00087   exit(1);
00088 }
00089 
00090 
00091 // this function is called each frame
00092 void glut_display (void)
00093 {
00094   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00095 
00096   // Setup the OpenGL viewpoint
00097   glMatrixMode(GL_PROJECTION);
00098   glPushMatrix();
00099   glLoadIdentity();
00100 
00101   glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0);
00102 
00103   if (g_draw_image && g_image_drawer)  g_image_drawer->draw();
00104   if (g_draw_depth && g_depth_drawer)  g_depth_drawer->draw();
00105 
00106   if (! g_users.empty() || ! g_hands.empty() ) {
00107     if (!g_pause) {
00108       for (UserMap::iterator i = g_users.begin(); i != g_users.end(); ++i) {
00109         i->second.skel_if->read();
00110         i->second.proj_if->read();
00111       }
00112       for (HandMap::iterator i = g_hands.begin(); i != g_hands.end(); ++i) {
00113         i->second.hand_if->read();
00114       }
00115     }
00116 
00117     // Process the data
00118     if (g_draw_skeleton)  g_skeleton_drawer->draw();
00119   }
00120 
00121   glutSwapBuffers();
00122 }
00123 
00124 
00125 void glut_idle()
00126 {
00127   if (g_quit)  clean_exit();
00128 
00129   g_obs->process_queue();
00130   g_hands_obs->process_queue();
00131 
00132   // Display the frame
00133   glutPostRedisplay();
00134 }
00135 
00136 void
00137 glut_keyboard(unsigned char key, int x, int y)
00138 {
00139   switch (key) {
00140   case 27:
00141   case 'q':
00142     clean_exit();
00143   case 'i':
00144     g_draw_image = ! g_draw_image;
00145     if (g_draw_image)  g_draw_depth = false;
00146     break;
00147   case 'd':
00148     g_draw_depth = ! g_draw_depth;
00149     if (g_draw_depth)  g_draw_image = false;
00150     break;
00151   case 's':
00152     g_draw_skeleton = ! g_draw_skeleton;
00153     break;
00154   case 'l':
00155     g_skeleton_drawer->toggle_print_state();
00156     break;
00157   case 'L':
00158     if (g_depth_drawer) g_depth_drawer->toggle_show_labels();
00159     break;
00160   case'p':
00161     g_pause = !g_pause;
00162     break;
00163   }
00164 }
00165 
00166 void glInit (int * pargc, char ** argv)
00167 {
00168   glutInit(pargc, argv);
00169   glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
00170   glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
00171   glutCreateWindow ("Fawkes Skeleton GUI");
00172   //glutFullScreen();
00173   glutSetCursor(GLUT_CURSOR_NONE);
00174 
00175   glutKeyboardFunc(glut_keyboard);
00176   glutDisplayFunc(glut_display);
00177   glutIdleFunc(glut_idle);
00178 
00179   glDisable(GL_DEPTH_TEST);
00180   glEnable(GL_TEXTURE_2D);
00181 
00182   glEnableClientState(GL_VERTEX_ARRAY);
00183   glDisableClientState(GL_COLOR_ARRAY);
00184 }
00185 
00186 int main(int argc, char **argv)
00187 {
00188   ArgumentParser argp(argc, argv, "hr:n::js");
00189 
00190   Thread::init_main();
00191   glInit(&argc, argv);
00192 
00193   std::string host = "localhost";
00194   unsigned short int port = 1910;
00195   if ( argp.has_arg("r") ) {
00196     argp.parse_hostport("r", host, port);
00197   }
00198 
00199   std::string fvhost = host;
00200   unsigned short int fvport = 2208;
00201   if ( argp.has_arg("n") ) {
00202     argp.parse_hostport("n", fvhost, fvport);
00203   }
00204 
00205   printf("Connecting to %s:%u\n", host.c_str(), port);
00206   try {
00207     g_rbb = new RemoteBlackBoard(host.c_str(), port);
00208   } catch (Exception &e) {
00209     e.print_trace();
00210     exit(-1);
00211   }
00212   g_obs = new SkelIfObserver(g_rbb, g_users);
00213   g_hands_obs = new HandIfObserver(g_rbb, g_hands);
00214 
00215   g_image_cam = NULL;
00216   g_depth_cam = NULL;
00217   g_label_cam = NULL;
00218   g_image_drawer = NULL;
00219 
00220   if (argp.has_arg("n") || argp.has_arg("s")) {
00221     if (argp.has_arg("n")) {
00222       g_image_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-image",
00223                                       argp.has_arg("j"));
00224       g_depth_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-depth");
00225       g_label_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-labels");
00226     } else {
00227       g_image_cam = new SharedMemoryCamera("openni-image");
00228       g_depth_cam = new SharedMemoryCamera("openni-depth");
00229       try {
00230         g_label_cam = new SharedMemoryCamera("openni-labels");
00231         g_label_cam->open();
00232         g_label_cam->start();
00233         if ((g_label_cam->pixel_width() != GL_WIN_SIZE_X) ||
00234             (g_label_cam->pixel_height() != GL_WIN_SIZE_Y)) {
00235           delete g_label_cam;
00236           g_label_cam = NULL;
00237           throw Exception("Invalid label cam");
00238         }
00239       } catch (Exception &e) {
00240         printf("Cannot open label cam, user tracker not running? "
00241                "Disabling labels.\n");
00242       }
00243     }
00244     g_image_cam->open();
00245     g_image_cam->start();
00246     g_depth_cam->open();
00247     g_depth_cam->start();
00248     if (g_label_cam) {
00249       g_label_cam->open();
00250       g_label_cam->start();
00251     }
00252 
00253     if ((g_image_cam->pixel_width() != GL_WIN_SIZE_X) ||
00254         (g_image_cam->pixel_height() != GL_WIN_SIZE_Y) ||
00255         (g_depth_cam->pixel_width() != GL_WIN_SIZE_X) ||
00256         (g_depth_cam->pixel_height() != GL_WIN_SIZE_Y) )
00257     {
00258       printf("Image size different from window size, closing camera");
00259       delete g_image_cam;
00260       delete g_depth_cam;
00261       g_image_cam = g_depth_cam = NULL;
00262     }
00263 
00264     g_image_drawer = new SkelGuiImageDrawer(g_image_cam);
00265     g_depth_drawer = new SkelGuiDepthDrawer(g_depth_cam, g_label_cam, 10000);
00266   }
00267 
00268   g_skeleton_drawer = new SkelGuiSkeletonDrawer(g_users, g_hands);
00269 
00270   glutMainLoop();
00271 }