gui.cpp

来自「移动机器人SLAM,参考一位外国人的硕士论文」· C++ 代码 · 共 255 行

/*
  slam3 (c) 2006 Kris Beevers

  This file is part of slam3.

  slam3 is free software; you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  slam3 is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
  License for more details.

  You should have received a copy of the GNU General Public License
  along with slam3; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include "slam.hpp"
#include "options.hpp"
#include <GL/glut.h>
#include <algorithm>

extern void process();

// in slam.cpp
extern pose_history_t gps_hist;
extern bool done;

// FIXME come up with a better way to pass this stuff in
#include "rbpf.hpp"
#ifdef GRIDSLAM // FIXME deal with these in slam_interface somehow
typedef slam_scan_match mapper_type;
//typedef slam_scan_match_2 mapper_type;
#else
typedef slam_interface mapper_type;
#endif
extern mapper_type mapper;

namespace gui {


const char *title_prefix = "SLAM2 - ";
int mouse_x, mouse_y;
bool paused = false, movie = false, mouse_left = false, mouse_right = false;
uint32_t redraw_freq = 1;

int viewport_w, viewport_h, viewport_w_orig, viewport_h_orig;
double workspace_w, workspace_h, scale = 1, trans_x, trans_y, extra_x, extra_y;

uint32_t ss_count = 0;
char ss_fn[256];


bool screenshot(const char *prefix)
{
  sprintf(ss_fn, "%s%08d.ppm", prefix, ss_count);
  ++ss_count;

  unsigned int size = 3 * viewport_w * viewport_h;
  unsigned char *buffer = new unsigned char[size];
  memset(buffer, 0, size);

  glPixelStorei(GL_PACK_ALIGNMENT, 1);
  glReadPixels(0, 0, viewport_w, viewport_h, GL_RGB, GL_UNSIGNED_BYTE, buffer);

  FILE *out = fopen(ss_fn, "wb");
  if(!out)
    return false;
  fprintf(out, "P6\n%d\n%d\n255\n", viewport_w, viewport_h);
  const unsigned char *pix;
  int y, x;
  // this will flip the y-axis
  for(y = viewport_h - 1; y >= 0; --y) {
    for(x = 0; x < int(viewport_h); ++x) {
      pix = &buffer[3*(y*viewport_w+x)];
      fwrite(pix, 3, 1, out);
    }
  }
  fclose(out);
  return true;
}

void idle()
{
  if(!paused) {
    for(uint32_t i = 0; i < redraw_freq; ++i)
      process();
    glutPostRedisplay();
  } else
    usleep(100000);
}

void display()
{
  glClearColor(0.7, 0.7, 0.7, 1);
  glClear(GL_COLOR_BUFFER_BIT);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(scale * (-workspace_w / 2 + trans_x),
	     scale * (workspace_w / 2 + trans_x + extra_x),
	     scale * (-workspace_h / 2 + trans_y),
	     scale * (workspace_h / 2 + trans_y + extra_y));

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  // because occupancy grid data is stored at a different orientation
  glRotatef(-90, 0, 0, 1);

  // axes
  glLineWidth(1);
  glColor3f(0.8,0.8,0.8);
  glBegin(GL_LINES);
  glVertex2d(-100000, 0); glVertex2d(100000, 0);
  glVertex2d(0, -100000); glVertex2d(0, 100000);
  glEnd();

  glColor3f(0.0, 0.25, 0.25);
  gps_hist.render(); // gps trajectory

  mapper.render();

  glutSwapBuffers();

  if(movie)
    screenshot("movie/");
}

void reshape(int width, int height)
{
  glViewport(0, 0, width, height);
  viewport_w = width;
  viewport_h = height;
  extra_x = workspace_w * (float(width) - viewport_w_orig) / float(viewport_w_orig);
  extra_y = workspace_h * (float(height) - viewport_h_orig) / float(viewport_h_orig);
}

void keyboard(uint8_t key, int x, int y)
{
  switch(key) {
  case 'Q': // quit
  case 'q':
    printf("\ngoodbye\n");
    if(!done)
      mapper.write_to_file("output");
    exit(0);

  case 'S': // screenshot
  case 's':
    screenshot("");
    printf("wrote %s\n", ss_fn);
    break;

  case 'M': // start/stop dump every screen (for movie)
  case 'm':
    movie = !movie;
    printf(movie ? "dumping every frame to movie/...\n" : "stopped dumping frames\n");
    break;

  case 'N': // process one step if paused
  case 'n':
    if(paused) {
      printf("processing one step, press SPACE for full throttle\n");
      process();
    }
    break;

  case ' ': // pause
    paused = !paused;
    printf(paused ? "\npaused, press SPACE to continue\n" : "unpaused\n");
    return; // no redisplay
  }
  glutPostRedisplay();
}

void mouse(int button, int state, int x, int y)
{
  mouse_x = x;
  mouse_y = y;

  if(state == GLUT_UP) {
    if(button == GLUT_LEFT_BUTTON) mouse_left = false;
    else if(button == GLUT_RIGHT_BUTTON) mouse_right = false;
  } else {
    if(button == GLUT_LEFT_BUTTON) mouse_left = true;
    else if(button == GLUT_RIGHT_BUTTON) mouse_right = true;
  }
}

void motion(int x, int y)
{
  int dx = x - mouse_x, dy = y - mouse_y;
  if(dx == 0 && dy == 0)
    return;

  if(mouse_left) { // pan
    trans_x -= workspace_w * float(dx) / float(viewport_w_orig);
    trans_y += workspace_h * float(dy) / float(viewport_h_orig);
  } else if(mouse_right) // zoom
    scale *= exp(0.05 * double(dy) + 0.01);

  mouse_x = x;
  mouse_y = y;

  if(mouse_left || mouse_right)
    glutPostRedisplay();
}

void special(int key, int x, int y)
{
  if(key == GLUT_KEY_HOME) {
    scale = 1.0;
    trans_x = trans_y = 0.0;
  }
  glutPostRedisplay();
}

void run(int argc, char **argv, const char *filename)
{
  // get commandline options that we use for rendering
  viewport_w = viewport_w_orig = std::max(options::quickget<int>("gui-width"), 1);
  viewport_h = viewport_h_orig = std::max(options::quickget<int>("gui-height"), 1);
  workspace_w = std::max(options::quickget<double>("workspace-width"), 0.1);
  workspace_h = std::max(options::quickget<double>("workspace-height"), 0.1);
  redraw_freq = std::max(options::quickget<uint32_t>("redraw-freq"), 1u);
  movie = options::quickget<bool>("movie");

  // set up renderer
  glutInit(&argc, argv);

  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
  glutInitWindowSize(viewport_w, viewport_h);

  char *title = new char[strlen(title_prefix) + strlen(filename) + 1];
  sprintf(title, "%s%s", title_prefix, filename);
  glutCreateWindow(title);

  glutDisplayFunc(display);
  glutReshapeFunc(reshape);
  glutKeyboardFunc(keyboard);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  glutSpecialFunc(special);
  glutIdleFunc(idle);

  glutMainLoop();
}

}; // namespace gui