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 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);
436 glutDisplayFunc(display);
void add_camera(std::string name, firevision::Camera *cam)
Add a camera from which to pull images.
bool parse_hostport(const char *argn, char **host, unsigned short int *port)
Parse host:port string.
Camera interface for image aquiring devices in FireVision.
const std::vector< const char *> & items() const
Get non-option items.
ObjectPositionInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
void process_queue()
Process internal queue.
virtual unsigned int pixel_width()=0
Width of image in pixels.
Draw body skeleton using OpenGL (3D).
Parse command line arguments.
void draw()
Draw skeletons.
static void init_main()
Initialize Thread wrapper instance for main thread.
virtual colorspace_t colorspace()=0
Colorspace of returned image.
void process_queue()
Process internal queue.
Skeleton interface observer.
Base class for exceptions in Fawkes.
virtual void capture()=0
Capture an image.
virtual void open()=0
Open the camera.
const unsigned char * buffer(std::string name)
Get buffer for specified camera.
void print_trace()
Prints trace to stderr.
PCL viewer transfer thread.
virtual unsigned char * buffer()=0
Get access to current image buffer.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
void lock_for_read()
Lock for reading.
virtual unsigned int pixel_height()=0
Height of image in pixels.
bool has_arg(const char *argn)
Check if argument has been supplied.
The BlackBoard abstract class.
virtual void start()=0
Start image transfer from the camera.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void start(bool wait=true)
Call this method to start the thread.
virtual void dispose_buffer()=0
Dispose current buffer.
virtual void close(Interface *interface)=0
Close interface.