PlatformUnitGuiPlugin.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "PlatformUnitGuiPlugin.h"
#include "PlatformUnitConfigDialog.h"
#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <Eigen/Geometry>
 
// Qt headers
#include <Qt>
#include <QtGlobal>
#include <QPushButton>
#include <QLabel>
#include <QLineEdit>
#include <QHBoxLayout>
 
//std
#include <RobotAPI/interface/core/RobotLocalization.h>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <memory>
#include <cmath>
 
using namespace armarx;
 
 
 
PlatformUnitGuiPlugin::PlatformUnitGuiPlugin()
{
    addWidget<PlatformUnitWidget>();
}
 
 
PlatformUnitWidget::PlatformUnitWidget() :
    platformUnitProxyName("PlatformUnit"),  // overwritten in loadSettings() anyway?
    platformName("Platform"),
    speedCtrl {nullptr},
    rotaCtrl {nullptr},
    ctrlEvaluationTimer {},
    platformRotation {0},
    platformMoves {false}
{
    // init gui
    ui.setupUi(getWidget());
    //init joystick controls
    std::unique_ptr<JoystickControlWidget> speed{new JoystickControlWidget{}};
    speedCtrl = speed.get();
    speedCtrl->setSteps(0);
    //use upper semicircle for rotation
    std::unique_ptr<JoystickControlWidget> rotat{new JoystickControlWidget{false}};
    rotaCtrl = rotat.get();
    rotaCtrl->setSteps(0);
    //add joystick controls
    ui.gridLayout_2->addWidget(rotat.release(), 2, 0, 1, 2);
    ui.gridLayout_3->addWidget(speed.release(), 2, 0, 1, 2);
 
    ctrlEvaluationTimer.setSingleShot(false);
    keyboardVelocityTimer.setInterval(50);
    stopPlatformTimer.setInterval(100);
    connect(&stopPlatformTimer, SIGNAL(timeout()), this, SLOT(stopControlTimer()));
    connect(&stopPlatformTimer, SIGNAL(timeout()), this, SLOT(stopPlatform()));
 
 
    connect(getWidget().data(), SIGNAL(commandKeyPressed(int)), this, SLOT(controlPlatformWithKeyboard(int)));
    connect(getWidget().data(), SIGNAL(commandKeyReleased(int)), this, SLOT(stopPlatformWithKeyboard(int)));
}
 
 
void PlatformUnitWidget::onInitComponent()
{
    usingProxy(platformUnitProxyName);
    usingTopic("GlobalRobotPoseLocalization");
    // ARMARX_INFO << "Listening on Topic: " << platformName + "State";
 
}
 
 
void PlatformUnitWidget::onConnectComponent()
{
    platformUnitProxy = getProxy<PlatformUnitInterfacePrx>(platformUnitProxyName);
    connectSlots();
}
 
 
void PlatformUnitWidget::onDisconnectComponent()
{
    stopControlTimer();
}
 
 
void PlatformUnitWidget::onExitComponent()
{
}
 
 
QPointer<QDialog> PlatformUnitWidget::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new PlatformUnitConfigDialog(parent);
    }
 
    dialog->ui->editPlatformName->setText(QString::fromStdString(platformName));
    return qobject_cast<PlatformUnitConfigDialog*>(dialog);
}
 
 
void PlatformUnitWidget::configured()
{
    platformUnitProxyName = dialog->finder->getSelectedProxyName().toStdString();
    platformName = dialog->ui->editPlatformName->text().toStdString();
}
 
 
void PlatformUnitWidget::loadSettings(QSettings* settings)
{
    platformUnitProxyName = settings->value("platformUnitProxyName", QString::fromStdString(platformUnitProxyName)).toString().toStdString();
    platformName = settings->value("platformName", QString::fromStdString(platformName)).toString().toStdString();
}
 
 
void PlatformUnitWidget::saveSettings(QSettings* settings)
{
    settings->setValue("platformUnitProxyName", QString::fromStdString(platformUnitProxyName));
    settings->setValue("platformName", QString::fromStdString(platformName));
}
 
 
void PlatformUnitWidget::connectSlots()
{
    connect(ui.buttonMoveToPosition, SIGNAL(clicked()), this, SLOT(moveTo()), Qt::UniqueConnection);
    connect(&ctrlEvaluationTimer, SIGNAL(timeout()), this, SLOT(controlTimerTick()), Qt::UniqueConnection);
    connect(&keyboardVelocityTimer, SIGNAL(timeout()), this, SLOT(keyboardVelocityControl()), Qt::UniqueConnection);
    connect(speedCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()), Qt::UniqueConnection);
    connect(speedCtrl, SIGNAL(pressed()), &keyboardVelocityTimer, SLOT(stop()), Qt::UniqueConnection);
    connect(rotaCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()), Qt::UniqueConnection);
    connect(rotaCtrl, SIGNAL(pressed()), &keyboardVelocityTimer, SLOT(stop()), Qt::UniqueConnection);
    connect(speedCtrl, SIGNAL(released()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
    connect(speedCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()), Qt::UniqueConnection);
    connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
    connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()), Qt::UniqueConnection);
    connect(ui.buttonStopPlatform, SIGNAL(pressed()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
}
 
 
void PlatformUnitWidget::moveTo()
{
    ARMARX_LOG << "Moving Platform";
    ::Ice::Float positionX = ui.editTargetPositionX->text().toFloat();
    ::Ice::Float positionY = ui.editTargetPositionY->text().toFloat();
    ::Ice::Float rotation = ui.editTargetRotation->text().toFloat();
    ::Ice::Float posAcc = 10.0f;
    ::Ice::Float rotAcc = 0.1f;
    platformUnitProxy->moveTo(positionX, positionY, rotation, posAcc, rotAcc);
}
 
 
void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
{
    ui.labelCurrentPositionX->setText(QString::number(x));
    ui.labelCurrentPositionY->setText(QString::number(y));
    ui.labelCurrentRotation->setText(QString::number(alpha));
}
 
 
void PlatformUnitWidget::setNewTargetPoseLabels(float x, float y, float alpha)
{
    ui.editTargetPositionX->setText(QString::number(x));
    ui.editTargetPositionY->setText(QString::number(y));
    ui.editTargetRotation->setText(QString::number(alpha));
}
 
 
void PlatformUnitWidget::startControlTimer()
{
    ctrlEvaluationTimer.start(CONTROL_TICK_RATE);  //tickrate in ms
}
 
 
void PlatformUnitWidget::stopControlTimer()
{
    ctrlEvaluationTimer.stop();
    speedCtrl->setNibble({0, 0});
    rotaCtrl->setNibble({0, 0});
}
 
 
void PlatformUnitWidget::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
{
    const Eigen::Isometry3f global_T_robot(transformStamped.transform);
    const float x = global_T_robot.translation().x();
    const float y = global_T_robot.translation().y();
    const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
 
    // moved to qt thread for thread safety
    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, yaw));
    platformRotation = yaw;
}
 
 
void PlatformUnitWidget::stopPlatform()
{
    platformUnitProxy->stopPlatform();
}
 
 
void PlatformUnitWidget::controlPlatformWithKeyboard(int key)
{
    pressedKeys.insert(key);
    if (!ctrlEvaluationTimer.isActive())
    {
        ctrlEvaluationTimer.start();
    }
    if (!keyboardVelocityTimer.isActive())
    {
        keyboardVelocityControl();
        keyboardVelocityTimer.start();
    }
}
 
 
void PlatformUnitWidget::stopPlatformWithKeyboard(int key)
{
    pressedKeys.remove(key);
 
    if (!keyboardVelocityTimer.isActive())
    {
        keyboardVelocityControl();
        keyboardVelocityTimer.start();
    }
}
 
 
void PlatformUnitWidget::keyboardVelocityControl()
{
    if (!pressedKeys.contains(Qt::Key_A) && !pressedKeys.contains(Qt::Key_D))
    {
        currentKeyboardVelocityX *= deceleration;
        if (fabs(currentKeyboardVelocityX) < 0.001)
        {
            currentKeyboardVelocityX = 0;
        }
    }
    if (!pressedKeys.contains(Qt::Key_W) && !pressedKeys.contains(Qt::Key_S))
    {
        currentKeyboardVelocityY *= deceleration;
        if (fabs(currentKeyboardVelocityY) < 0.001)
        {
            currentKeyboardVelocityY = 0;
        }
    }
    if (!pressedKeys.contains(Qt::Key_Q) && !pressedKeys.contains(Qt::Key_E))
    {
        currentKeyboardVelocityAlpha *= deceleration;
        if (fabs(currentKeyboardVelocityAlpha) < 0.001)
        {
            currentKeyboardVelocityAlpha = 0;
        }
    }
 
    for (auto key : pressedKeys)
    {
        switch (key)
        {
            case Qt::Key_Q:
                currentKeyboardVelocityAlpha -= acceleration;
                break;
            case Qt::Key_E:
                currentKeyboardVelocityAlpha += acceleration;
                break;
            case Qt::Key_W:
                currentKeyboardVelocityY -= acceleration;
                break;
            case Qt::Key_S:
                currentKeyboardVelocityY += acceleration;
                break;
            case Qt::Key_A:
                currentKeyboardVelocityX -= acceleration;
                break;
            case Qt::Key_D:
                currentKeyboardVelocityX += acceleration;
                break;
            default:
                break;
        }
    }
 
    currentKeyboardVelocityAlpha = std::max(-1.f, currentKeyboardVelocityAlpha);
    currentKeyboardVelocityAlpha = std::min(1.f, currentKeyboardVelocityAlpha);
    currentKeyboardVelocityX = std::max(-1.f, currentKeyboardVelocityX);
    currentKeyboardVelocityX = std::min(1.f, currentKeyboardVelocityX);
    currentKeyboardVelocityY = std::max(-1.f, currentKeyboardVelocityY);
    currentKeyboardVelocityY = std::min(1.f, currentKeyboardVelocityY);
 
    float y = sin(acos(currentKeyboardVelocityAlpha));
    speedCtrl->setNibble(QPointF(currentKeyboardVelocityX, currentKeyboardVelocityY));
    rotaCtrl->setNibble(QPointF(currentKeyboardVelocityAlpha, -y));
}
 
 
QPointer<QWidget> PlatformUnitWidget::getWidget()
{
    if (!__widget)
    {
        __widget = new KeyboardPlatformHookWidget();
    }
 
    return __widget;
}
 
 
void PlatformUnitWidget::controlTimerTick()
{
    float translationFactor = ui.maxTranslationSpeed->value();
    float rotationFactor = ui.maxRotationSpeed->value() * -1;
    float rotationVel =  rotaCtrl->getRotation() / M_PI_2 * rotationFactor;
    ARMARX_INFO << deactivateSpam(0.5)
                << "Translation speed: (" << speedCtrl->getPosition().x() * translationFactor
                << ", " << speedCtrl->getPosition().y() * translationFactor << ")"
                << ", \t rotation speed: "  << (rotationVel);
 
    platformUnitProxy->move(speedCtrl->getPosition().x() * translationFactor,
                            -1 * speedCtrl->getPosition().y() * translationFactor,
                            rotationVel);
 
    if (speedCtrl->getPosition().x() == 0
        && speedCtrl->getPosition().y() == 0
        && rotaCtrl->getRotation() == 0)
    {
        stopControlTimer();
    }
}
 
 
void KeyboardPlatformHookWidget::keyPressEvent(QKeyEvent* event)
{
    switch (event->key())
    {
        case Qt::Key_A:
        case Qt::Key_W:
        case Qt::Key_S:
        case Qt::Key_D:
        case Qt::Key_Q:
        case Qt::Key_E:
            if (!event->isAutoRepeat())
            {
                emit commandKeyPressed(event->key());
            }
    }
    QWidget::keyPressEvent(event);
}
 
 
void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
{
    switch (event->key())
    {
        case Qt::Key_A:
        case Qt::Key_W:
        case Qt::Key_S:
        case Qt::Key_D:
        case Qt::Key_Q:
        case Qt::Key_E:
            if (!event->isAutoRepeat())
            {
                emit commandKeyReleased(event->key());
            }
    }
    QWidget::keyReleaseEvent(event);
}