programs/UrgViewer/UrgViewerWindow.cpp

Go to the documentation of this file.

#include "UrgViewerWindow.h"
#include "UrgDrawWidget.h"
#include "RangeSensorConnectThread.h"
#include "SerialConnectionWidget.h"
#include "EthernetConnectionWidget.h"
#include "CaptureSettingWidget.h"
#include "LengthViewWidget.h"
#include "mUrgDevice.h"
#include "RangeSensorParameter.h"
#include "SerialDevice.h"
#include "TcpipSocket.h"
#include "FindComPorts.h"
#include "UrgUsbCom.h"
#include "ticks.h"
#include "MonitorModeManager.h"
#include "MonitorEventScheduler.h"
#include "Lock.h"
#include "LockGuard.h"
#include "log_printf.h"
#include "codedString.h"
#include "split.h"
#include <QTimer>
#include <QDateTime>
#include <QShortcut>
#include <QMessageBox>
#include <QSettings>
#include <QCloseEvent>

using namespace qrk;
using namespace std;


//#define DEBUG 1


namespace
{
    const char* Version = "1.0.4";

    const char* Organization = "Hokuyo LTD.";
    const char* Application = "URG Viewer";
    const char* UrgDefaultAddress = "192.168.0.10";

    enum {
        UrgDefaultPort = 10940,
        InvalidTimestamp = -1,
    };
}


struct UrgViewerWindow::pImpl
{
    UrgViewerWindow* widget_;

    mUrgDevice urg_;
    SerialDevice* serial_connection_;
    TcpipSocket ethernet_connection_;

    FindComPorts com_finder_;
    UrgUsbCom urg_usb_;

    CaptureSettingWidget capture_setting_widget_;
    LengthViewWidget length_view_widget_;

    UrgDrawWidget urg_draw_widget_;
    SerialConnectionWidget serial_connection_widget_;
    EthernetConnectionWidget ethernet_connection_widget_;
    RangeSensorConnectThread connect_thread_;
    bool draw_intensity_;
    Lock urg_mutex_;

    QTimer capture_timer_;
    long last_timestamp_;
    QDateTime last_record_time_;

    RangeSensorParameter parameter_;
    CaptureSettings* settings_;

    bool set_length_view_;
    vector<int> draw_line_indexes_;
    vector<Point<long> > draw_line_points_;


    pImpl(int argc, char *argv[], UrgViewerWindow* widget)
        : widget_(widget), urg_(argc, argv), serial_connection_(NULL),
          capture_setting_widget_(urg_), connect_thread_(urg_),
          draw_intensity_(false), last_timestamp_(InvalidTimestamp),
          last_record_time_(QDateTime::currentDateTime()),
          settings_(NULL), set_length_view_(false)
    {
        com_finder_.addBaseName("/dev/ttyACM");
        com_finder_.addBaseName("/dev/tty.usbmodem");

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

        ethernet_connection_widget_.setPort(UrgDefaultPort);

        for (int i = 1; i < argc; ++i) {
            if (! strcmp("-i", argv[i])) {
                draw_intensity_ = true;
            }
        }

        if (draw_intensity_) {
            capture_setting_widget_.setIntensityMode();
        }
    }


    ~pImpl(void)
    {
        delete settings_;
    }


    void initializeForm(void)
    {
        // タイトルにバージョン情報を付加
        const QString title = widget_->windowTitle() + " " + Version;
        widget_->setWindowTitle(title);

        // ウィジットの配置
        widget_->dummy_label_2_->hide();
        widget_->main_layout_->addWidget(&urg_draw_widget_);
        widget_->connect_layout_->addWidget(&serial_connection_widget_);
        widget_->connect_layout_->addWidget(&ethernet_connection_widget_);

        // メニュー項目
        connect(widget_->action_quit_, SIGNAL(triggered()),
                widget_, SLOT(close()));
        connect(widget_->action_about_, SIGNAL(triggered()),
                widget_, SLOT(aboutApplication()));
        connect(widget_->action_zoomIn_, SIGNAL(triggered()),
                widget_, SLOT(zoomLarger()));
        connect(widget_->action_zoomOut_, SIGNAL(triggered()),
                widget_, SLOT(zoomSmaller()));
        connect(widget_->action_capture_setting_, SIGNAL(triggered()),
                widget_, SLOT(showCaptureSettingWidget()));
        connect(widget_->action_length_view_, SIGNAL(triggered()),
                widget_, SLOT(showLengthViewWidget()));
        connect(widget_->action_serial_, SIGNAL(triggered()),
                widget_, SLOT(selectSerial()));
        connect(widget_->action_ethernet_, SIGNAL(triggered()),
                widget_, SLOT(selectEthernet()));

        // URG デバイス
        connect(&capture_timer_, SIGNAL(timeout()),
                widget_, SLOT(captureHandler()));

        // Serial 接続
        connect(&serial_connection_widget_, SIGNAL(rescanRequest()),
                widget_, SLOT(rescanPressed()));
        connect(&serial_connection_widget_,
                SIGNAL(connectRequest(bool, const std::string&)),
                widget_, SLOT(connectPressed(bool, const std::string&)));
        connect(&connect_thread_, SIGNAL(connected(bool)),
                widget_, SLOT(connectDevice(bool)));

        // Ethernet 接続
        connect(&ethernet_connection_widget_,
                SIGNAL(connectRequest(bool, const std::string&,
                                      unsigned short)),
                widget_, SLOT(connectPressed(bool, const std::string&,
                                             unsigned short)));

        // ステータスバー表示
        connect(&urg_draw_widget_, SIGNAL(position(bool, long, long)),
                widget_, SLOT(updateStatusBar(bool, long, long)));

        // キー割り付け
        (void) new QShortcut(Qt::Key_Return, widget_, SLOT(initializeView()));
        (void) new QShortcut(Qt::Key_Less, widget_, SLOT(zoomSmaller()));
        (void) new QShortcut(Qt::Key_Comma, widget_, SLOT(zoomSmaller()));
        (void) new QShortcut(Qt::Key_Greater, widget_, SLOT(zoomLarger()));
        (void) new QShortcut(Qt::Key_Period, widget_, SLOT(zoomLarger()));

        // 外部 Widget からのシグナル
        connect(&capture_setting_widget_,
                SIGNAL(setCaptureSettings(const qrk::CaptureSettings&)),
                widget_,
                SLOT(receiveCaptureSettings(const qrk::CaptureSettings&)));
        connect(&capture_setting_widget_, SIGNAL(reconnectRequest()),
                widget_, SLOT(receiveReconnectRequest()));
        connect(&length_view_widget_, SIGNAL(lengthViewRequest()),
                widget_, SLOT(receiveLengthViewRequest()));

        connect(&capture_setting_widget_,
                SIGNAL(widgetClose(const std::string&)),
                widget_, SLOT(receiveWidgetClose(const std::string&)));
        connect(&length_view_widget_, SIGNAL(widgetClose(const std::string&)),
                widget_, SLOT(receiveWidgetClose(const std::string&)));
        connect(&length_view_widget_, SIGNAL(quit()), widget_, SLOT(close()));
        connect(&capture_setting_widget_, SIGNAL(quit()),
                widget_, SLOT(close()));

        connect(&length_view_widget_,
                SIGNAL(drawLineRequest(const std::vector<int>&)),
                widget_, SLOT(receiveDrawLine(const std::vector<int>&)));

        connect(widget_, SIGNAL(reconnectCompleted()),
                &capture_setting_widget_, SLOT(reconnectCompleted()));

        // キー割り付け
        (void) new QShortcut(Qt::CTRL + Qt::Key_W, widget_, SLOT(close()));

        widget_->rescanPressed();
    }


    void saveSettings(void)
    {
        QSettings settings(Organization, Application);

        settings.setValue("geometry", widget_->saveGeometry());
        settings.setValue("LengthView_geometry",
                          length_view_widget_.saveGeometry());
        settings.setValue("CaptureSetting_geometry",
                          capture_setting_widget_.saveGeometry());

        if (MonitorModeManager::object()->mode() != MonitorModeManager::Play) {
            settings.setValue("LengthView_showed",
                              widget_->action_length_view_->isChecked());
            settings.setValue("CaptureSetting_showed",
                              widget_->action_capture_setting_->isChecked());
        }

        bool use_serial = serial_connection_widget_.isVisible();
        settings.setValue("serial_connection", use_serial);

        settings.setValue("address",
                          ethernet_connection_widget_.address().c_str());
        settings.setValue("port", ethernet_connection_widget_.port());
    }


    void loadSettings(void)
    {
        QSettings settings(Organization, Application);

        widget_->restoreGeometry(settings.value("geometry").toByteArray());
        length_view_widget_.
            restoreGeometry(settings.value("LengthView_geometry").toByteArray());
        capture_setting_widget_.
            restoreGeometry(settings.value("CaptureSetting_geometry").toByteArray());

        if (MonitorModeManager::object()->mode() != MonitorModeManager::Play) {
            if (settings.value("LengthView_showed", false).toBool() == true) {
                activateWidget(&length_view_widget_,
                               widget_->action_length_view_);
            }
            if (settings.value("CaptureSetting_showed",
                               false).toBool() == true) {
                activateWidget(&capture_setting_widget_,
                               widget_->action_capture_setting_);
            }
        }

        bool use_serial = settings.value("serial_connection", true).toBool();
        if (use_serial) {
            widget_->selectSerial();
        } else {
            widget_->selectEthernet();
        }

        string address =
            settings.value("address",
                           UrgDefaultAddress).toString().toStdString();
        ethernet_connection_widget_.setAddress(address);
        unsigned short port = settings.value("port", UrgDefaultPort).toInt();
        ethernet_connection_widget_.setPort(port);
    }


    void connectDevice(bool connected)
    {
        last_timestamp_ = InvalidTimestamp;
        serial_connection_widget_.setConnected(connected);
        ethernet_connection_widget_.setConnected(connected);
        capture_setting_widget_.setConnected(connected);
        if (! connected) {
            // エラーメッセージのポップアップ表示
            QString error_message = localedToUnicode(urg_.what());
            QMessageBox::warning(widget_,
                                 tr("Connection error"), error_message);
            return;
        }

        // パラメータ情報が与えられていない場合、センサ情報を反映させる
        parameter_ = urg_.parameter();
        if (! settings_) {
            settings_ = initializeCaptureSettings(parameter_.area_min,
                                                  parameter_.area_max);
            capture_setting_widget_.loadPressed();
        }

        size_t type_length = parameter_.model.find('(');
        string urg_type = parameter_.model.substr(0, type_length);
        bool cluster_enable =
            ! (draw_intensity_ && urg_type.compare("UTM-30LX"));
        if (draw_intensity_) {
            capture_setting_widget_.setIntensityMode(cluster_enable);
        }

        receiveReconnectRequest();
        length_view_widget_.setMinDistance(urg_.minDistance());

        // URG 設定
        urg_.setTimestamp(ticks());
        int scan_msec = urg_.scanMsec();
        urg_draw_widget_.setDrawPeriod(scan_msec);

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


    CaptureSettings* initializeCaptureSettings(int first_index, int last_index)
    {
        CaptureSettings* settings = new CaptureSettings;
        settings->type = MD;
        settings->capture_first = first_index;
        settings->capture_last = last_index;
        settings->skip_lines = 1;
        settings->skip_frames = 0;
        settings->remain_times = 0;
        settings->data_byte = 3;

        return settings;
    }

    void receiveReconnectRequest(void)
    {
        capture_timer_.stop();
        urg_draw_widget_.clear();

        if (draw_intensity_) {
            urg_.setCaptureMode(IntensityCapture);

        } else if (settings_->type == MD) {
            urg_.setCaptureMode(AutoCapture);

        } else if (settings_->type == GD) {
            urg_.setCaptureMode(ManualCapture);
        }

        urg_.setCaptureRange(settings_->capture_first, settings_->capture_last);
        urg_.setCaptureSkipLines(settings_->skip_lines);

        if (settings_->type != GD) {
            urg_.setCaptureFrameInterval(settings_->skip_frames);
            urg_.setCaptureTimes(settings_->remain_times);
        }

        if (! urg_.isConnected()) {
            // 未接続ならば再開させない
            return;
        }

        setConnectionMenuEnabled(false);
        capture_timer_.start();
    }


    void disconnectDevice(void)
    {
        // データの取得停止
        widget_->scans_label_->setText("");
        capture_timer_.stop();
        urg_draw_widget_.clear();
        capture_setting_widget_.setApplyEnabled(true);

        serial_connection_widget_.setConnected(false);
        ethernet_connection_widget_.setConnected(false);
        capture_setting_widget_.setConnected(false);
        setConnectionMenuEnabled(true);
    }


    void setConnectionMenuEnabled(bool enabled)
    {
        widget_->action_serial_->setEnabled(enabled);
        widget_->action_ethernet_->setEnabled(enabled);
    }


    void captureHandler(void)
    {
        LockGuard guard(urg_mutex_);

        vector<long> data;
        vector<long> intensity_data;
        long timestamp = 0;

        RangeCaptureMode capture_mode = urg_.captureMode();
        int n = 0;
        if (capture_mode != IntensityCapture) {
            n = urg_.capture(data, &timestamp);

        } else {
            n = urg_.captureWithIntensity(data, intensity_data, &timestamp);
        }
        if (n <= 0) {
            return;
        }

#if defined(DEBUG)
        // !!! debug message
        QDateTime current = QDateTime::currentDateTime();
        if (last_record_time_.secsTo(current) >= 60) {
            log_printf("%s\n",
                       current.toString(Qt::ISODate).toStdString().c_str());
            last_record_time_ = current;
        }
#endif

        // update "scans interval"
        int interval_msec = timestamp - last_timestamp_;
        if (last_timestamp_ != InvalidTimestamp) {
            widget_->updateScansInterval(interval_msec);
        }
        last_timestamp_ = timestamp;

        // setUrgData() を呼び出すとデータは空になるので、
        // ここで LengthView 用のデータ格納を行う
        if (set_length_view_) {
            set_length_view_ = false;

            length_view_widget_.setLength(data);
            if (capture_mode == IntensityCapture) {
                length_view_widget_.setIntensity(intensity_data);
            }
        }

        // 補助線の登録
        for (vector<int>::iterator it = draw_line_indexes_.begin();
             it != draw_line_indexes_.end(); ++it) {
            double radian = urg_.index2rad(*it) + (M_PI/2.0);
            int x = static_cast<int>(100 * 1000 * cos(radian));
            int y = static_cast<int>(100 * 1000 * sin(radian));
            draw_line_points_.push_back(Point<long>(x, y));
        }
        urg_draw_widget_.setDrawColor(Color(0.0, 1.0, 0.0, 1.0));
        urg_draw_widget_.setUrgData(draw_line_points_, timestamp);

        // 描画データの登録
        if (capture_mode == IntensityCapture) {
            urg_draw_widget_.setUrgIntensityData(intensity_data,
                                                 &urg_, timestamp);
        }
        urg_draw_widget_.setDrawColor(Color(0.2f, 0.2f, 1.0f, 0.6f));
        urg_draw_widget_.setUrgData(data, &urg_, timestamp);

        // 一定間隔での描画
        // !!! FPS を定義する
        // !!! 可能ならば、URG のタイムスタンプを利用する
        urg_draw_widget_.redraw();

        if (settings_->type != GD) {
            if ((settings_->remain_times > 0) &&
                (urg_.remainCaptureTimes() == 0)) {
                // 回数指定のデータ取得のときは、指定回数後に取得を終了する
                capture_timer_.stop();
                capture_setting_widget_.setApplyEnabled(true);
            }
        }
    }


    void setPlayConnection(void)
    {
        // 接続操作を disable にする
        serial_connection_widget_.setEnabled(false);
        ethernet_connection_widget_.setEnabled(false);

        // 接続処理
        QTimer::singleShot(1000, widget_, SLOT(autoPlay()));
    }


    void activateWidget(QWidget* widget, QAction* action)
    {
        widget->show();
        widget->raise();
        action->setChecked(true);
    }


    void useSerialConnection(bool checked)
    {
        // メニューのチェックボックスの変更
        widget_->action_serial_->setChecked(checked);
        widget_->action_ethernet_->setChecked(! checked);

        // 接続用 Widget の変更
        // ウィンドウが広がらないように、表示されている方を消してから追加する
        if (checked) {
            ethernet_connection_widget_.setEnabled(false);
            ethernet_connection_widget_.hide();
            serial_connection_widget_.show();
            serial_connection_widget_.setEnabled(true);
        } else {
            serial_connection_widget_.setEnabled(false);
            serial_connection_widget_.hide();
            ethernet_connection_widget_.show();
            ethernet_connection_widget_.setEnabled(true);
        }
    }
};


UrgViewerWindow::UrgViewerWindow(int argc, char *argv[])
    : pimpl(new pImpl(argc, argv, this))
{
    setupUi(this);
    pimpl->initializeForm();
    pimpl->loadSettings();

    if (MonitorModeManager::object()->mode() == MonitorModeManager::Play) {
        // 接続操作を disable にして、接続を行っておく
        pimpl->setPlayConnection();

        // 取得範囲の設定ウィンドウは表示させない
        menu_Window_->setEnabled(false);
    }
}


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


void UrgViewerWindow::rescanPressed(void)
{
    vector<string> devices;
    pimpl->com_finder_.find(devices);

    for (vector<string>::iterator it = devices.begin();
         it != devices.end(); ++it) {
        if (pimpl->urg_usb_.isUsbCom(it->c_str())) {
            *it = *it + " [URG]";
        }
    }
    pimpl->serial_connection_widget_.setDevices(devices);
    if (! devices.empty()) {
        pimpl->serial_connection_widget_.setFocus();
    }
}


void UrgViewerWindow::connectPressed(bool connection,
                                     const string& device)
{
    LockGuard guard(pimpl->urg_mutex_);

    if (connection) {
        // URG との接続
        pimpl->connect_thread_.setConnectSettings(device, 115200);
        pimpl->connect_thread_.start();

        vector<string> tokens;
        size_t n = split(tokens, device, "/");
        if (n > 0) {
            string base_name = tokens[n - 1] + "_error_log.txt";
            log_setName(base_name.c_str());
        }

    } else {
        // 切断
        pimpl->disconnectDevice();
        pimpl->urg_.disconnect();
    }
}


void UrgViewerWindow::connectPressed(bool connection,
                                     const std::string& address,
                                     unsigned short port)
{
    LockGuard guard(pimpl->urg_mutex_);

    if (connection) {
        // URG との接続
        pimpl->connect_thread_.setConnectSettings(address, port);
        pimpl->connect_thread_.start();

    } else {
        // 切断
        pimpl->disconnectDevice();
        pimpl->urg_.disconnect();
    }
}


void UrgViewerWindow::connectDevice(bool connection)
{
    pimpl->connectDevice(connection);
}


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


void UrgViewerWindow::initializeView(void)
{
    pimpl->urg_draw_widget_.initializeView();
}


void UrgViewerWindow::updateStatusBar(bool active, long x_mm, long y_mm)
{
    if (! active) {
        statusBar()->clearMessage();
        return;
    }

    QString message = QString("X: %1 [mm], Y: %2 [mm]").arg(x_mm).arg(y_mm);
    statusBar()->showMessage(message, 60 * 1000);
}


void UrgViewerWindow::updateScansInterval(int msec)
{
    scans_label_->setText(QString(tr("%1 [msec] (scans interval)")).arg(msec));
}


void UrgViewerWindow::aboutApplication(void)
{
    QString message =
        "<h2>URG Viewer " + QString(Version) + "</h2>"
        "<p>Demo Application for URG sensor</p>"
        "<p>Report bugs to &lt;satofumi@users.sourceforge.jp&gt;</p>";

    QMessageBox::about(this, tr("About URG Viewer"), message);
}


void UrgViewerWindow::autoPlay(void)
{
    pimpl->connect_thread_.start();
}


void UrgViewerWindow::zoomSmaller(void)
{
    pimpl->urg_draw_widget_.updateZoomRatio(+1);
}


void UrgViewerWindow::zoomLarger(void)
{
    pimpl->urg_draw_widget_.updateZoomRatio(-1);
}


void UrgViewerWindow::closeEvent(QCloseEvent* event)
{
    pimpl->capture_setting_widget_.hide();
    pimpl->length_view_widget_.hide();

    pimpl->capture_timer_.stop();

    LockGuard guard(pimpl->urg_mutex_);
    pimpl->urg_.disconnect();
    pimpl->connect_thread_.terminate();
    pimpl->connect_thread_.wait(100);

    event->accept();

    // デストラクタ内で呼び出したいが、
    // exit() を呼ぶとデストラクタが呼び出されないのでここで呼び出す
    pimpl->saveSettings();

    exit(0);
}


void UrgViewerWindow::showCaptureSettingWidget(void)
{
    pimpl->activateWidget(&pimpl->capture_setting_widget_,
                          action_capture_setting_);
}


void UrgViewerWindow::receiveCaptureSettings(const CaptureSettings& settings)
{
    if (! pimpl->settings_) {
        int dummy = -1;
        pimpl->settings_ = pimpl->initializeCaptureSettings(dummy, dummy);
    }
    *pimpl->settings_ = settings;
}


void UrgViewerWindow::receiveReconnectRequest(void)
{
    pimpl->receiveReconnectRequest();
    emit reconnectCompleted();
}


void UrgViewerWindow::showLengthViewWidget(void)
{
    pimpl->activateWidget(&pimpl->length_view_widget_,
                          action_length_view_);
}


void UrgViewerWindow::receiveLengthViewRequest(void)
{
    pimpl->set_length_view_ = true;
}


void UrgViewerWindow::receiveWidgetClose(const string& widget_name)
{
    if (! widget_name.compare("CaptureSettingWidget")) {
        action_capture_setting_->setChecked(false);

    } else if (! widget_name.compare("LengthViewWidget")) {
        action_length_view_->setChecked(false);
    }
}


void UrgViewerWindow::receiveDrawLine(const vector<int>& indexes)
{
    pimpl->draw_line_indexes_ = indexes;
}


void UrgViewerWindow::selectSerial(void)
{
    pimpl->useSerialConnection(true);

    // Serial 用の Connection を登録
    if (pimpl->serial_connection_) {
        pimpl->urg_.setConnection(pimpl->serial_connection_);
    }
}


void UrgViewerWindow::selectEthernet(void)
{
    pimpl->useSerialConnection(false);

    // Ethernet 用の Connection を登録
    pimpl->urg_.setConnection(&pimpl->ethernet_connection_);
}