23 #include "skel_drawer.h"
24 #include "trackball.h"
25 #include "transfer_thread.h"
27 #include <GL/freeglut.h>
28 #include <blackboard/remote.h>
29 #include <core/threading/thread.h>
30 #include <fvcams/net.h>
31 #include <fvcams/shmem.h>
32 #include <fvutils/base/types.h>
33 #include <fvutils/color/conversions.h>
34 #include <fvutils/ipc/shm_image.h>
35 #include <interfaces/ObjectPositionInterface.h>
36 #include <plugins/openni/utils/hand_if_observer.h>
37 #include <plugins/openni/utils/skel_if_observer.h>
38 #include <utils/math/angle.h>
39 #include <utils/system/argparser.h>
43 #define GL_WIN_SIZE_X 800
44 #define GL_WIN_SIZE_Y 800
47 using namespace fawkes::openni;
48 using namespace firevision;
51 Camera * g_image_cam = NULL;
52 unsigned char * g_rgb_buf = NULL;
54 const unsigned char *g_image_buf = NULL;
57 GLdouble g_pan_x = 0.0;
58 GLdouble g_pan_y = 0.0;
59 GLdouble g_pan_z = 0.0;
60 GLint g_mouse_state = -1;
61 GLint g_mouse_button = -1;
65 std::list<ObjectPositionInterface *> g_obj_ifs;
79 glRotatef(90., 0, 0, 1);
80 glRotatef(45., 0, 1, 0);
82 glScalef(3.0, 3.0, 3.0);
88 if (!g_transfer_thread) {
109 g_transfer_thread->
unlock();
113 rotate_scale_matrix();
116 const unsigned int width = g_pcl_cam->
pixel_width();
121 if (g_transfer_thread) {
126 unsigned char *rgb = g_rgb_buf;
128 for (
unsigned int h = 0; h < height; ++h) {
129 for (
unsigned int w = 0; w < width; ++w, rgb += 3, ++pcl) {
132 if ((p.
x != 0) || (p.
y != 0) || (p.
z != 0)) {
133 glColor3f(rgb[0] / 255., rgb[1] / 255., rgb[2] / 255.);
134 glVertex3f(p.
x, p.
y, p.
z);
140 for (
unsigned int h = 0; h < height; ++h) {
141 for (
unsigned int w = 0; w < width; ++w, ++pcl) {
144 if ((p.
x != 0) || (p.
y != 0) || (p.
z != 0)) {
145 glVertex3f(p.
x, p.
y, p.
z);
151 if (g_transfer_thread) {
152 g_transfer_thread->
unlock();
160 glColor3f(1.0, 0, 0);
162 glColor3f(0.0, 0, 1.0);
163 for (
int i = -90; i <= 90; i += 10) {
167 glColor3f(1.0, 1.0, 1.0);
171 if (!g_transfer_thread) {
179 glRotatef(90., 0, 0, 1);
180 glRotatef(45., 0, 1, 0);
182 glScalef(3.0, 3.0, 3.0);
187 std::list<ObjectPositionInterface *>::iterator i;
188 for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) {
190 if ((*i)->is_visible()) {
191 glVertex4f((*i)->relative_x(), (*i)->relative_y(), (*i)->relative_z(), 1.0);
194 glColor3f(1.0, 1.0, 1.0);
200 mouse(
int button,
int state,
int x,
int y)
202 tbMouse(button, state, x, y);
204 g_mouse_state = state;
205 g_mouse_button = button;
207 if (state == GLUT_DOWN && button == GLUT_LEFT_BUTTON) {
208 GLdouble model[4 * 4];
209 GLdouble proj[4 * 4];
212 glGetDoublev(GL_MODELVIEW_MATRIX, model);
213 glGetDoublev(GL_PROJECTION_MATRIX, proj);
214 glGetIntegerv(GL_VIEWPORT, view);
215 gluProject((GLdouble)x, (GLdouble)y, 0.0, model, proj, view, &g_pan_x, &g_pan_y, &g_pan_z);
217 (GLdouble)x, (GLdouble)y, g_pan_z, model, proj, view, &g_pan_x, &g_pan_y, &g_pan_z);
229 if (g_mouse_state == GLUT_DOWN && g_mouse_button == GLUT_LEFT_BUTTON) {
230 GLdouble model[4 * 4];
231 GLdouble proj[4 * 4];
234 glGetDoublev(GL_MODELVIEW_MATRIX, model);
235 glGetDoublev(GL_PROJECTION_MATRIX, proj);
236 glGetIntegerv(GL_VIEWPORT, view);
237 gluProject((GLdouble)x, (GLdouble)y, 0.0, model, proj, view, &g_pan_x, &g_pan_y, &g_pan_z);
239 (GLdouble)x, (GLdouble)y, g_pan_z, model, proj, view, &g_pan_x, &g_pan_y, &g_pan_z);
247 reshape(
int width,
int height)
249 tbReshape(width, height);
251 glViewport(0, 0, width, height);
264 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
267 glTranslatef(g_pan_x, g_pan_y, 0.0);
276 if (!g_obj_ifs.empty()) {
284 rotate_scale_matrix();
285 g_skel_drawer->
draw();
290 printf(
"Interface read failed, closing");
291 std::list<ObjectPositionInterface *>::iterator i;
292 for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) {
318 tbInit(GLUT_MIDDLE_BUTTON);
321 tbAnimate(GL_TRUE, idle);
330 glEnable(GL_DEPTH_TEST);
333 glMatrixMode(GL_PROJECTION);
334 gluPerspective( 60.0,
338 glMatrixMode(GL_MODELVIEW);
353 std::string host =
"localhost";
355 unsigned short int port = 1910;
358 printf(
"Connecting to %s:%u\n", host.c_str(), port);
362 const std::vector<const char *> &items = argp.
items();
363 for (
unsigned int i = 0; i < items.size(); ++i) {
365 g_obj_ifs.push_back(obj_if);
377 std::string fvhost = host;
379 unsigned short int fvport = 2208;
381 g_pcl_cam =
new NetworkCamera(fvhost.c_str(), fvport,
"openni-pointcloud-xyz");
386 g_transfer_thread->
add_camera(
"openni-pointcloud-xyz", g_pcl_cam);
388 g_pcl_buf = (
const pcl_point_t *)g_transfer_thread->
buffer(
"openni-pointcloud-xyz");
394 g_image_cam->
start();
396 g_transfer_thread->
add_camera(
"openni-image-rgb", g_image_cam);
397 g_image_buf = g_transfer_thread->
buffer(
"openni-image-rgb");
400 g_transfer_thread->
start();
410 g_image_cam->
start();
411 g_image_buf = g_image_cam->
buffer();
418 main(
int argc,
char **argv)
423 glutInit(&argc, argv);
424 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
425 glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
426 glutCreateWindow(
"Fawkes OpenNI PCL Viewer");
427 glutReshapeFunc(reshape);
428 glutMouseFunc(mouse);
429 glutMotionFunc(motion);
430 glutDisplayFunc(display);