programs/MultiUrgViewer/UrgStateWidget.cpp

Go to the documentation of this file.

#include "UrgStateWidget.h"
#include "MultiUrgViewerWidget.h"
#include "SerialConnectionWidget.h"
#include "DropRateWidget.h"
#include "DrawWidget.h"
#include "UrgDevice.h"
#include "UrgUtils.h"
#include "MovingAverage.h"
#include "ticks.h"
#include <QTimer>

using namespace qrk;
using namespace std;


namespace
{
  enum {
    AverageTimes = 40,
  };
}


struct UrgStateWidget::pImpl
{
  MultiUrgViewerWidget* parent_;
  UrgStateWidget* widget_;
  SerialConnectionWidget connection_widget_;
  DropRateWidget drop_rate_widget_;
  DrawWidget* draw_widget_;

  UrgDevice urg_;
  string device_;
  long serial_id_;
  QTimer capture_timer_;
  int scan_msec_;
  long prev_timestamp_;

  MovingAverage<int> capture_times_;


  pImpl(MultiUrgViewerWidget* parent, UrgStateWidget* widget)
    : parent_(parent), widget_(widget), draw_widget_(NULL),
      serial_id_(0), prev_timestamp_(0), capture_times_(AverageTimes)
  {
    capture_times_.setAverageValue(100);
  }


  void initializeForm(void)
  {
    // Widget の配置
    widget_->connection_layout_->addWidget(&connection_widget_);
    widget_->drop_rate_layout_->addWidget(&drop_rate_widget_);
    widget_->connection_dummy_label_->hide();
    widget_->drop_rate_dummy_label_->hide();

    // イベントの接続
    connect(&connection_widget_, SIGNAL(rescanRequest()),
            parent_, SLOT(rescanDevices()));
    connect(&connection_widget_,
            SIGNAL(connectRequest(bool, const std::string&)),
            widget_, SLOT(connectDevice(bool, const std::string&)));
    connect(widget_, SIGNAL(deleteUrgState(UrgStateWidget*)),
            parent_, SLOT(deleteUrgState(UrgStateWidget*)));

    connect(&capture_timer_, SIGNAL(timeout()),
            widget_, SLOT(captureHandler()));

    connect(widget_->view_button_, SIGNAL(clicked()),
            widget_, SLOT(setViewer()));

    setEnabled(false);
  }


  void setEnabled(bool enable)
  {
    widget_->view_button_->setEnabled(enable);
    drop_rate_widget_.setEnabled(enable);
  }


  void connectDevice(const string& device)
  {
    // 接続時
    bool ret = urg_.connect(device.c_str());
    connection_widget_.setConnected(ret);
    if (! ret) {
      return;
    }

    device_ = device;
    parent_->addUrgState();
    parent_->rescanDevices();

    // シリアル ID の登録
    serial_id_ = urgSerialId<UrgDevice>(&urg_);
    QString title = serialId();

    widget_->groupbox_->setTitle(title);
    setEnabled(true);

    urg_.setCaptureMode(AutoCapture);
    prev_timestamp_ = ticks();
    urg_.setTimestamp(prev_timestamp_);
    scan_msec_ = urg_.scanMsec();

    // データの取得開始
    capture_timer_.setInterval(scan_msec_ / 2);
    capture_timer_.start();
  }


  QString serialId(void)
  {
    return QString("Serial ID : %1").arg(serial_id_);
  }


  void captureHandler(void)
  {
    vector<long> data;
    long timestamp = 0;
    int n = urg_.capture(data, &timestamp);
    if (n > 0) {
      int drop_times =
        static_cast<int>((3.0 + timestamp - prev_timestamp_) / scan_msec_) - 1;

      capture_times_.push_back(100);
      for (int i = 0; i < drop_times; ++i) {
        capture_times_.push_back(0);
      }
      prev_timestamp_ = timestamp;

      int average = capture_times_.average();
      drop_rate_widget_.setDropRate(average);

      // 描画データの表示
      if (draw_widget_) {
        draw_widget_->setUrgData(data, &urg_, timestamp, widget_);
      }
    }
  }
};


UrgStateWidget::UrgStateWidget(MultiUrgViewerWidget* parent)
  : QWidget(parent), pimpl(new pImpl(parent, this))
{
  setupUi(this);
  pimpl->initializeForm();
}


UrgStateWidget::~UrgStateWidget(void)
{
}


void UrgStateWidget::connectDevice(bool connection, const string& device)
{
  if (connection) {
    // 接続時
    pimpl->connectDevice(device);
  } else {
    // 切断時
    emit deleteUrgState(this);
  }
}


void UrgStateWidget::captureHandler(void)
{
  pimpl->captureHandler();
}


void UrgStateWidget::setViewer(void)
{
  DrawWidget* draw_widget = pimpl->parent_->drawWidget();
  pimpl->draw_widget_ = draw_widget;
  draw_widget->show();
  draw_widget->setTitle(pimpl->serialId(), this);
}


void UrgStateWidget::setDevices(const vector<string>& devices)
{
  pimpl->connection_widget_.setDevices(devices);
}


string UrgStateWidget::device(void) const
{
  return pimpl->device_;
}


bool UrgStateWidget::isConnected(void) const
{
  return pimpl->urg_.isConnected();
}