Fawkes API
Fawkes Development Version
|
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 }