programs/UrgScanner/UrgScannerWindow.cpp

Go to the documentation of this file.

#include <QShortcut>
#include <QTimer>
#include <QSettings>
#include <QFileDialog>
#include <QWheelEvent>
#include <cmath>
#include "UrgScannerWindow.h"
#include "UrgDataDraw.h"
#include "WiiJoystick.h"
#include "FindComPorts.h"
#include "UrgUsbCom.h"
#include "UrgDevice.h"
#include "ConvertStdStringPath.h"

using namespace qrk;
using namespace std;


struct UrgScannerWindow::pImpl
{
  WiiJoystick wii_;
  UrgDevice urg_;
  long* urg_data_;
  size_t urg_data_max_;
  UrgDataDraw* data_draw_widget_;
  QTimer* redraw_timer_;
  bool save_ok_;
  UrgUsbCom urg_usb_;
  FindComPorts urg_finder_;


  pImpl(UrgScannerWindow* parent)
    : urg_data_(NULL), urg_data_max_(1),
      data_draw_widget_(new UrgDataDraw(parent)),
      redraw_timer_(new QTimer(parent)), save_ok_(false)
  {
    urg_finder_.addBaseName("/dev/ttyACM");
    urg_finder_.addBaseName("/dev/tty.usbmodem");

    urg_finder_.addDriverName("URG Series USB Device Driver");
    urg_finder_.addDriverName("URG-X002 USB Device Driver");
  }


  void loadSettings(UrgScannerWindow* parent)
  {
    QSettings settings("Hokuyo LTD.", "URG Scanner");

#ifdef WINDOWS_OS
    // ポート選択用のコンボボックスの設定を読み出し
    parent->com_combobox_->
      setCurrentIndex(settings.value("port_index", 0).toInt());
#endif

    // H 型 URG かどうかの選択を読み出し
    bool h_type = settings.value("h_type", false).toBool();
    parent->mirror_checkbox_->setChecked(h_type);
    data_draw_widget_->setTypeH(h_type);

    // 全面を表示するかの選択を読み出し
    bool front_only = settings.value("front_only", false).toBool();
    parent->front_only_checkbox_->setChecked(front_only);
    data_draw_widget_->setFrontOnly(front_only);

    // 高感度モードの選択を読み出し
    bool hs_mode = settings.value("hs_mode", false).toBool();
    parent->hs_mode_checkbox_->setChecked(hs_mode);

    // 強度データ表示の選択を読み出し
    bool intensity_mode = settings.value("intensity_mode", false).toBool();
    parent->intensity_checkbox_->setChecked(intensity_mode);
  }


  void saveSettings(UrgScannerWindow* parent) {

    QSettings settings("Hokuyo LTD.", "URG Scanner");

#ifdef WINDOWS_OS
    settings.setValue("port_index",
                      parent->com_combobox_->currentIndex());
#endif
    settings.setValue("h_type", parent->mirror_checkbox_->checkState());
    settings.setValue("front_only", parent->front_only_checkbox_->checkState());
    settings.setValue("hs_mode", parent->hs_mode_checkbox_->checkState());
    settings.setValue("intensity_mode",
                      parent->intensity_checkbox_->checkState());
  }

  // フォームの初期化
  void initializeForm(UrgScannerWindow* parent)
  {
    parent->com_combobox_->clear();
    vector<string> devices;
    urg_finder_.find(devices);
    for (vector<string>::iterator it = devices.begin();
         it != devices.end(); ++it) {
      if (urg_usb_.isUsbCom(it->c_str())) {
        *it = *it + " [URG]";
      }
      parent->com_combobox_->addItem(it->c_str());
    }

    // 描画ウィジットの配置
    parent->verticalLayout_tools_->addWidget(data_draw_widget_);

    // 接続ボタンの処理
    connect(parent->connect_button_, SIGNAL(clicked(bool)),
            parent, SLOT(connectHandler(bool)));

    // URG データの再描画
    connect(redraw_timer_, SIGNAL(timeout()), parent, SLOT(redrawHandler()));

    // H 型かどうか
    connect(parent->mirror_checkbox_, SIGNAL(clicked(bool)),
            data_draw_widget_, SLOT(setTypeH(bool)));
    data_draw_widget_->
      setTypeH((parent->mirror_checkbox_->checkState() != Qt::Checked)
               ? false : true);

    // 前方のみの取得
    connect(parent->front_only_checkbox_, SIGNAL(clicked(bool)),
            data_draw_widget_, SLOT(setFrontOnly(bool)));
    data_draw_widget_->
      setFrontOnly((parent->front_only_checkbox_->checkState() != Qt::Checked)
                   ? false : true);

    parent->hs_mode_checkbox_->hide();


    // 強度データ
    connect(parent->intensity_checkbox_, SIGNAL(clicked(bool)),
            parent, SLOT(meHandler(bool)));

    // 拡大率の変更
    connect(parent->magnify_slidebar_, SIGNAL(valueChanged(int)),
            data_draw_widget_, SLOT(magnifyChanged(int)));

    // 拡大率について、フォームの初期値を代入
    data_draw_widget_->magnifyChanged(parent->magnify_slidebar_->value());

    // RETURN キーで、視点を書記化
    (void) new QShortcut(Qt::Key_Return, data_draw_widget_, SLOT(resetView()));

    // VRML の保存、読み込みイベント
    connect(parent->load_button_, SIGNAL(clicked()),
            parent, SLOT(loadVrml()));
    connect(parent->save_button_, SIGNAL(clicked()),
            parent, SLOT(saveVrml()));
  }

  void clearForm(UrgScannerWindow* parent) {
    static_cast<void>(parent);

    // !!! Window のメッセージ欄に表示する
    //parent->battery_label_->setText(tr("battery:  --[%]"));

    // データ描画ウィンドウから、測定中データを消去
    data_draw_widget_->clearCaptureData();
  }

  // Z 軸, X 軸 の順に回転させることを想定している
  void getWiiRotate(Point3d<int>& wii_rotate)
  {
    if (wii_.isConnected()) {

      // 加速度情報の取得
      Point3d<double> acc;
      wii_.acceleration(acc);

      // 回転角度の計算
      double length = sqrt((acc.x * acc.x) + (acc.z * acc.z));
      double x_rad = atan2(-acc.y, length);
      double z_rad = atan2(acc.z, acc.x);

      wii_rotate.x = -static_cast<int>(180 * z_rad / M_PI) + 90;
      wii_rotate.y = -static_cast<int>(180 * x_rad / M_PI);
      wii_rotate.z = 0;

    } else {
      wii_rotate.x = 0;
    }
  }
};


UrgScannerWindow::UrgScannerWindow(void) : pimpl(new pImpl(this))
{
  setupUi(this);

  pimpl->initializeForm(this);
  pimpl->clearForm(this);

  pimpl->loadSettings(this);

  // Ctrl-q で終了させる
  (void) new QShortcut(Qt::CTRL + Qt::Key_Q, this, SLOT(close()));
}


UrgScannerWindow::~UrgScannerWindow(void)
{
  pimpl->saveSettings(this);
}


void UrgScannerWindow::connectHandler(bool checked)
{
  // !!! 接続時には「Press wiimote 1 + 2 buttons.」と表示させる

  if (checked) {
    if (! pimpl->wii_.connect()) {
      // !!! メッセージボックスにエラーを表示する
      fprintf(stderr, "WiiJoystick::connect: %s\n", pimpl->wii_.what());
      // return;

      // !!! Wii がなくても表示はできるようにする
    }
    enum { MovingAverageSize = 12 };
    pimpl->wii_.setAccelerationAverageSize(MovingAverageSize);

    // URG への接続処理
    if (com_combobox_->count() == 0) {
      return;
    }
    string device_name = com_combobox_->currentText().toStdString();
    string device = device_name.substr(0, device_name.find(' '));

    if (! pimpl->urg_.connect(device.c_str(), 115200)) {
      // !!! メッセージボックスにエラーを表示する
      fprintf(stderr, "UrgDevice::connect: %s\n", pimpl->urg_.what());
      return;
    }
    pimpl->urg_.setCaptureMode(AutoCapture);

    // Intensity モードに設定
    bool me_on = intensity_checkbox_->isChecked() ? true : false;
    if (! meHandler(me_on)) {
      // 設定に失敗したら、Intensity のチェックボックスを無効にする
      intensity_checkbox_->setEnabled(false);
    }

    // 表示名の変更
    connect_button_->setText(tr("Disconnect"));

    if (! pimpl->urg_data_) {
      // 受信バッファの初期化
      pimpl->urg_data_max_ = pimpl->urg_.maxScanLines();
      pimpl->urg_data_ = new long[pimpl->urg_data_max_];
      //pimpl->urg_.setCaptureRange(0, pimpl->urg_data_max_);
    }

    // インターバル間隔は、URG の更新周期とする
    size_t scan_msec = pimpl->urg_.scanMsec();
    pimpl->redraw_timer_->start(scan_msec);

  } else {
    // 離されたとき disconnect()
    connect_button_->setText(tr("Connect"));
    pimpl->redraw_timer_->stop();

    if (pimpl->wii_.isConnected()) {
      pimpl->wii_.disconnect();
    }
    pimpl->urg_.disconnect();
    intensity_checkbox_->setEnabled(true);

    pimpl->clearForm(this);
  }
}


// 再描画
void UrgScannerWindow::redrawHandler(void)
{
  // Wii 姿勢の取得
  Point3d<int> wii_rotate;
  pimpl->getWiiRotate(wii_rotate);

  // 記録するかどうか、の取得
  bool record = pimpl->wii_.isButtonPressed(WiiJoystick::BUTTON_B);
  if (record && (! pimpl->save_ok_)) {
    pimpl->save_ok_ = true;
    save_button_->setEnabled(true);
  }

  // 現在のデータを表示するかどうか
  bool no_plot =
    pimpl->wii_.isButtonPressed(WiiJoystick::BUTTON_1) ||
    pimpl->wii_.isButtonPressed(WiiJoystick::BUTTON_2);

  // データの再描画
  pimpl->data_draw_widget_->redraw(pimpl->urg_, wii_rotate, record, no_plot);
}


// Intensity モードの設定
bool UrgScannerWindow::meHandler(bool checked)
{
  if (! pimpl->urg_.isConnected()) {
    // 接続されていなければ、戻る
    return true;
  }

  // Intensity モードを変更する
  pimpl->redraw_timer_->stop();
  RangeCaptureMode mode = checked ? IntensityCapture : AutoCapture;
  pimpl->urg_.setCaptureMode(mode);
  pimpl->data_draw_widget_->setIntensityMode(checked);
  pimpl->redraw_timer_->start();

  return true;
}


void UrgScannerWindow::loadVrml(void)
{
  QString fileName =
    QFileDialog::getOpenFileName(this, tr("Open VRML file."), ".",
                                 tr("VRML file (*.wrl)"));

  if (! fileName.isEmpty()) {
    // 日本語を含むパスへの対処
    string std_path = qrk::toStdStringPath(fileName);

    pimpl->data_draw_widget_->loadVrml(std_path);
  }
}


void UrgScannerWindow::saveVrml(void)
{
  // !!! 全ての保存を SaveAs として扱う

  QString fileName =
    QFileDialog::getSaveFileName(this, tr("Save VRML file."), ".",
                                 tr("VRML file (*.wrl)"));

  if (! fileName.isEmpty()) {
    // 日本語を含むパスへの対処
    string std_path = qrk::toStdStringPath(fileName);

    pimpl->data_draw_widget_->saveVrml(std_path);
  }
}


// ホイールによる拡大縮小の操作
void UrgScannerWindow::wheelEvent(QWheelEvent* event)
{
  int degrees = event->delta() / 8;
  int steps = degrees / 15;

  int add_value = ((steps > 0) ? +1 : -1) * magnify_slidebar_->singleStep();
  magnify_slidebar_->setValue(magnify_slidebar_->value() + add_value);

  event->accept();
}