23 #include "trackball.h"
24 #include "skel_drawer.h"
25 #include "transfer_thread.h"
26 #include <plugins/openni/utils/skel_if_observer.h>
27 #include <plugins/openni/utils/hand_if_observer.h>
29 #include <core/threading/thread.h>
30 #include <utils/math/angle.h>
31 #include <fvcams/net.h>
32 #include <fvcams/shmem.h>
33 #include <utils/system/argparser.h>
34 #include <fvutils/ipc/shm_image.h>
35 #include <fvutils/color/conversions.h>
36 #include <fvutils/base/types.h>
38 #include <blackboard/remote.h>
39 #include <interfaces/ObjectPositionInterface.h>
44 #define GL_WIN_SIZE_X 800
45 #define GL_WIN_SIZE_Y 800
47 using namespace fawkes;
48 using namespace fawkes::openni;
49 using namespace firevision;
52 Camera *g_image_cam = NULL;
53 unsigned char *g_rgb_buf = NULL;
55 const unsigned char *g_image_buf = NULL;
58 GLdouble g_pan_x = 0.0;
59 GLdouble g_pan_y = 0.0;
60 GLdouble g_pan_z = 0.0;
61 GLint g_mouse_state = -1;
62 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);
89 if (! g_transfer_thread) {
100 convert(g_image_cam->
colorspace(), RGB, g_image_buf, g_rgb_buf,
102 g_transfer_thread->
unlock();
106 rotate_scale_matrix();
109 const unsigned int width = g_pcl_cam->
pixel_width();
114 if (g_transfer_thread) {
119 unsigned char *rgb = g_rgb_buf;
121 for (
unsigned int h = 0; h < height; ++h) {
122 for (
unsigned int w = 0; w < width; ++w, rgb += 3, ++pcl) {
125 if ((p.
x != 0) || (p.
y != 0) || (p.
z != 0)) {
126 glColor3f(rgb[0] / 255.,rgb[1] / 255.,rgb[2] / 255.);
127 glVertex3f(p.
x, p.
y, p.
z);
133 for (
unsigned int h = 0; h < height; ++h) {
134 for (
unsigned int w = 0; w < width; ++w, ++pcl) {
137 if ((p.
x != 0) || (p.
y != 0) || (p.
z != 0)) {
138 glVertex3f(p.
x, p.
y, p.
z);
144 if (g_transfer_thread) {
145 g_transfer_thread->
unlock();
153 glColor3f(1.0, 0, 0);
155 glColor3f(0.0, 0, 1.0);
156 for (
int i = -90; i <= 90; i += 10) {
161 glColor3f(1.0, 1.0, 1.0);
165 if (! g_transfer_thread) {
174 glRotatef(90., 0, 0, 1);
175 glRotatef(45., 0, 1, 0);
177 glScalef(3.0, 3.0, 3.0);
182 std::list<ObjectPositionInterface *>::iterator i;
183 for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) {
185 if ((*i)->is_visible()) {
186 glVertex4f((*i)->relative_x(),
192 glColor3f(1.0, 1.0, 1.0);
199 mouse(
int button,
int state,
int x,
int y)
201 tbMouse(button, state, x, y);
203 g_mouse_state = state;
204 g_mouse_button = button;
206 if (state == GLUT_DOWN && button == GLUT_LEFT_BUTTON) {
211 glGetDoublev(GL_MODELVIEW_MATRIX, model);
212 glGetDoublev(GL_PROJECTION_MATRIX, proj);
213 glGetIntegerv(GL_VIEWPORT, view);
214 gluProject((GLdouble)x, (GLdouble)y, 0.0,
216 &g_pan_x, &g_pan_y, &g_pan_z);
217 gluUnProject((GLdouble)x, (GLdouble)y, g_pan_z,
219 &g_pan_x, &g_pan_y, &g_pan_z);
232 if (g_mouse_state == GLUT_DOWN && g_mouse_button == GLUT_LEFT_BUTTON) {
237 glGetDoublev(GL_MODELVIEW_MATRIX, model);
238 glGetDoublev(GL_PROJECTION_MATRIX, proj);
239 glGetIntegerv(GL_VIEWPORT, view);
240 gluProject((GLdouble)x, (GLdouble)y, 0.0,
242 &g_pan_x, &g_pan_y, &g_pan_z);
243 gluUnProject((GLdouble)x, (GLdouble)y, g_pan_z,
245 &g_pan_x, &g_pan_y, &g_pan_z);
254 reshape(
int width,
int height)
256 tbReshape(width, height);
258 glViewport(0, 0, width, height);
272 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
275 glTranslatef(g_pan_x, g_pan_y, 0.0);
284 if (! g_obj_ifs.empty()) {
292 rotate_scale_matrix();
293 g_skel_drawer->
draw();
298 printf(
"Interface read failed, closing");
299 std::list<ObjectPositionInterface *>::iterator i;
300 for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) {
326 tbInit(GLUT_MIDDLE_BUTTON);
329 tbAnimate(GL_TRUE, idle);
338 glEnable(GL_DEPTH_TEST);
341 glMatrixMode(GL_PROJECTION);
342 gluPerspective( 60.0,
345 glMatrixMode(GL_MODELVIEW);
346 gluLookAt(0.0, 0.0, 5.0,
354 std::string host =
"localhost";
355 unsigned short int port = 1910;
359 printf(
"Connecting to %s:%u\n", host.c_str(), port);
363 const std::vector< const char * > &items = argp.
items();
364 for (
unsigned int i = 0; i < items.size(); ++i) {
367 g_obj_ifs.push_back(obj_if);
379 std::string fvhost = host;
380 unsigned short int fvport = 2208;
384 g_pcl_cam =
new NetworkCamera(fvhost.c_str(), fvport,
"openni-pointcloud-xyz");
389 g_transfer_thread->
add_camera(
"openni-pointcloud-xyz", g_pcl_cam);
391 g_pcl_buf = (
const pcl_point_t *)g_transfer_thread->
buffer(
"openni-pointcloud-xyz");
394 g_image_cam =
new NetworkCamera(fvhost.c_str(), fvport,
"openni-image-rgb",
397 g_image_cam->
start();
398 g_rgb_buf = malloc_buffer(RGB, g_image_cam->
pixel_width(),
400 g_transfer_thread->
add_camera(
"openni-image-rgb", g_image_cam);
401 g_image_buf = g_transfer_thread->
buffer(
"openni-image-rgb");
404 g_transfer_thread->
start();
414 g_image_cam->
start();
415 g_image_buf = g_image_cam->
buffer();
416 g_rgb_buf = malloc_buffer(RGB, g_image_cam->
pixel_width(),
424 main(
int argc,
char **argv)
429 glutInit(&argc, argv);
430 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
431 glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
432 glutCreateWindow(
"Fawkes OpenNI PCL Viewer");
433 glutReshapeFunc(reshape);
434 glutMouseFunc(mouse);
435 glutMotionFunc(motion);
436 glutDisplayFunc(display);