PlatformUnitGuiPlugin.cpp

#include "PlatformUnitGuiPlugin.h"
#include "PlatformUnitConfigDialog.h"
#include "ui_PlatformUnitConfigDialog.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>

// Qt headers
#include <Qt>
#include <QtGlobal>
#include <QPushButton>
#include <QLabel>
#include <QLineEdit>
#include <QHBoxLayout>

//std
#include <memory>

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);
}


void PlatformUnitWidget::onInitComponent()
{
    usingProxy(platformUnitProxyName);
    usingTopic(platformName + "State");
    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()));
    connect(&ctrlEvaluationTimer, SIGNAL(timeout()), this, SLOT(controlTimerTick()));
    connect(speedCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()));
    connect(rotaCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()));
    connect(speedCtrl, SIGNAL(released()), this, SLOT(stopPlatform()));
    connect(speedCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()));
    connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopPlatform()));
    connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()));
    connect(ui.buttonStopPlatform, SIGNAL(pressed()), this, SLOT(stopPlatform()));

}

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();
}

void PlatformUnitWidget::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
{
    // moved to qt thread for thread safety
    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPlatformPositionX), Q_ARG(float, currentPlatformPositionY), Q_ARG(float, currentPlatformRotation));
    platformRotation = currentPlatformRotation;
}

void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
{
    // moved to qt thread for thread safety
    QMetaObject::invokeMethod(this, "setNewTargetPoseLabels",
                              Q_ARG(float, newPlatformPositionX),
                              Q_ARG(float, newPlatformPositionY),
                              Q_ARG(float, newPlatformRotation));
}

void PlatformUnitWidget::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
{

}

void PlatformUnitWidget::stopPlatform()
{
    platformUnitProxy->move(0, 0, 0);
}



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) << speedCtrl->getPosition().x()*translationFactor << ", " << speedCtrl->getPosition().y()*translationFactor;
    ARMARX_INFO << deactivateSpam(0.5) << "Rotation Speed: "  << (rotationVel);
    platformUnitProxy->move(speedCtrl->getPosition().x()*translationFactor, -1 * speedCtrl->getPosition().y()*translationFactor, rotationVel);

}

Q_EXPORT_PLUGIN2(robotapi_gui_PlatformUnitGuiPlugin, PlatformUnitGuiPlugin)