MMMPlayerGuiPlugin.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 "MMMPlayerGuiPlugin.h"
#include "MMMPlayerConfigDialog.h"
#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerConfigDialog.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <IceUtil/Time.h>
 
 
// Qt headers
#include <QInputDialog>
#include <Qt>
#include <QtGlobal>
#include <QPushButton>
#include <QComboBox>
#include <QMenu>
 
#include <filesystem>
#include <QMessageBox>
 
#include <RobotComponents/components/MMMPlayer/MMMPlayer.h>
#include <RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h>
using namespace armarx;
 
#define PROJECT_DEFAULT "Armar4"
#define CONFIG_FILE_DEFAULT "data/MMM/punching.xml"
 
#define POSITION_CONTROL 0
#define VELOCITY_CONTROL 1
 
MMMPlayerGuiPlugin::MMMPlayerGuiPlugin()
{
    addWidget<MMMPlayerWidget>();
}
 
MMMPlayerWidget::MMMPlayerWidget()
{
    // init gui
    ui.setupUi(getWidget());
    fileDialog2 = new QFileDialog();
    fileDialog2->setModal(true);
    fileDialog2->setFileMode(QFileDialog::ExistingFiles);
    QStringList fileTypes;
    fileTypes << tr("XML (*.xml)") << tr("All Files (*.*)");
    fileDialog2->setNameFilters(fileTypes);
 
    jointList = new QListWidget();
    updateTimer = new QTimer(this);
 
}
 
/*
QPointer<QDialog> MMMPlayerWidget::getConfigDialog(QWidget* parent)
{
    if(!dialog)
    {
        dialog = new MMMLoaderConfigDialog(parent);
        std::filesystem::path dir(CONFIG_FILE_DEFAULT);
 
        dialog->fileDialog->setDirectory(QString::fromStdString(dir.remove_filename().string()));
        dialog->ui->editConfigFile->setText(QString::fromStdString(CONFIG_FILE_DEFAULT));
    }
    return qobject_cast<MMMLoaderConfigDialog*>(dialog);
}
 
void MMMPlayerWidget::configured()
{
    ARMARX_VERBOSE << "MMMPlayerWidget::configured()";
    configFile = dialog->ui->editConfigFile->text().toStdString();
    ARMARX_VERBOSE << "config file " + configFile;
}
*/
 
 
void MMMPlayerWidget::onInitComponent()
{
    isComponentCreated = false;
    if (dialog->needtoCreate)
    {
 
        std::string kinematicUnitName = dialog->kinematicUnitComponentProxyFinder->getSelectedProxyName().trimmed().toStdString();
        std::string kinematicTopic = dialog->kinematicTopicName;
 
        Ice::StringSeq pros;
        std::string packageName("RobotComponents");
        armarx::CMakePackageFinder finder(packageName);
 
        std::string appPath = finder.getBinaryDir() + "/TrajectoryPlayerAppRun";
 
        pros.push_back(appPath);
        Ice::PropertiesPtr properties = Ice::createProperties(pros);
 
        properties->setProperty("ArmarX.TrajectoryPlayer.KinematicUnitName", kinematicUnitName);
        properties->setProperty("ArmarX.TrajectoryPlayer.KinematicTopicName", kinematicTopic);
 
        //        TrajectoryPlayerPtr mplptr = Component::create<TrajectoryPlayer>(properties, "TrajectoryPlayer");
        //        getArmarXManager()->addObject(mplptr);
 
 
        appPath = finder.getBinaryDir() + "/MMMPlayerAppRun";
        MMMPlayerPtr mptr = Component::create<MMMPlayer>(Ice::createProperties(), "MMMPlayer");
        getArmarXManager()->addObject(mptr);
 
        isComponentCreated = true;
 
    }
 
    usingProxy(dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
    usingProxy(dialog->trajPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
    //    ui.frameSlider->setEnabled(false);
 
 
    connectSlots();
}
 
 
void MMMPlayerWidget::onConnectComponent()
{
    MMMLoader = getProxy<MMMPlayerInterfacePrx>(dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
    trajPlayer = getProxy<TrajectoryPlayerInterfacePrx>(dialog->trajPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
 
    configFile = MMMLoader->getMotionPath();
    ui.editConfigFile->setText(QString::fromStdString(configFile));
    std::filesystem::path dir(configFile);
    fileDialog2->setDirectory(QString::fromStdString(dir.remove_filename().string()));
    setupMotionList();
 
    if (MMMLoader->isMotionLoaded())
    {
        setupJointList();
        loadTrajPlayer();
    }
 
    ui.frameSlider->blockSignals(true);
    ui.frameSlider->setMaximum(100);
    on_loopPlayback_toggled(false);
    //on_enableRobotPoseUnit_toggled(false);
}
 
void MMMPlayerWidget::onExitComponent()
{
}
 
void MMMPlayerWidget::onDisconnectComponent()
{
}
 
bool MMMPlayerWidget::onClose()
{
 
    if (getIceManager())
    {
        getIceManager()->removeObject("MMMPlayerConfigDialog");
 
        if (isComponentCreated)
        {
            getIceManager()->removeObject(dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
        }
    }
 
 
    return ArmarXComponentWidgetController::onClose();
 
 
}
 
 
void MMMPlayerWidget::loadSettings(QSettings* settings)
{
}
 
 
void MMMPlayerWidget::saveSettings(QSettings* settings)
{
}
 
void MMMPlayerWidget::loadTrajPlayer()
{
    TrajectoryBasePtr t = MMMLoader->getJointTraj();
    TrajectoryPtr tr = IceInternal::Handle<Trajectory>::dynamicCast(t);
    trajPlayer->loadJointTraj(t);
    trajPlayer->loadBasePoseTraj(MMMLoader->getBasePoseTraj());
    //ARMARX_INFO << "spinbox info: " << ui.speedSpinBox->value();
    //ARMARX_INFO << "end traj time: " << trajPlayer->getTrajEndTime();
    //    trajPlayer->setEndTime(trajPlayer->getTrajEndTime() / ui.speedSpinBox->value());
}
 
 
void MMMPlayerWidget::connectSlots()
{
    connect(ui.initializeButton, SIGNAL(clicked()), this, SLOT(on_initializeButton_clicked()));
    connect(ui.startButton, SIGNAL(clicked()), this, SLOT(on_startButton_clicked()));
    connect(ui.pauseButton, SIGNAL(clicked()), this, SLOT(on_pauseButton_clicked()));
    connect(ui.stopButton, SIGNAL(clicked()), this, SLOT(on_stopButton_clicked()));
    connect(ui.previewButton, SIGNAL(clicked()), this, SLOT(on_previewButton_clicked()));
 
    connect(ui.btnSelectConfig, SIGNAL(clicked()), fileDialog2, SLOT(show()));
    connect(ui.selectJoints, SIGNAL(clicked()), jointList, SLOT(show()));
    connect(fileDialog2, SIGNAL(accepted()), this, SLOT(configFileSelected()));
    connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateSlider()));
    //    connect(ui.frameSlider, SIGNAL(valueChanged(int)), this, SLOT(setFrameNumber(int)));
    //    connect(ui.loopPlayback, SIGNAL(toggled(bool)), this, SLOT(on_loopPlayback_toggled(bool)));
    connect(ui.loopPlayback, SIGNAL(toggled(bool)), this, SLOT(on_loopPlayback_toggled(bool)));
    connect(ui.enableRobotPoseUnit, SIGNAL(toggled(bool)), this, SLOT(on_enableRobotPoseUnit_toggled(bool)));
    connect(ui.speedSpinBox, SIGNAL(valueChanged(double)), this, SLOT(on_spinBoxFPS_valueChanged(double)));
    connect(ui.controlMode, SIGNAL(currentIndexChanged(int)), this, SLOT(on_controlMode_changed(int)));
    connect(jointList, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(jointListChanged(QListWidgetItem*)));
}
 
void armarx::MMMPlayerWidget::on_initializeButton_clicked()
{
    // set position control mode and move to first frame as initial pose
 
    trajPlayer->resetTrajectoryPlayer(true);
    ui.startButton->setEnabled(true);
    ui.previewButton->setEnabled(true);
 
    updateSlider();
}
 
void armarx::MMMPlayerWidget::setSpeed(double value)
{
    trajPlayer->setEndTime(trajPlayer->getTrajEndTime() / value);
 
}
 
void armarx::MMMPlayerWidget::on_startButton_clicked()
{
    trajPlayer->setIsPreview(false);
    setSpeed(ui.speedSpinBox->value());
    on_controlMode_changed(ui.controlMode->currentIndex());
    bool started = trajPlayer->startTrajectoryPlayer();
 
 
    if (started)
    {
        updateTimer->start(100);
        ui.initializeButton->setDisabled(started);
        ui.startButton->setDisabled(started);
        ui.previewButton->setDisabled(started);
        ui.motionName->setDisabled(started);
        ui.controlMode->setDisabled(started);
        ui.pauseButton->setEnabled(started);
        ui.pauseButton->setText("Pause");
        ui.btnSelectConfig->setEnabled(false);
        ui.speedSpinBox->setDisabled(started);
        ui.stopButton->setEnabled(started);
        ui.frameSlider->setDisabled(started);
        ui.frameSlider->blockSignals(true);
 
        ui.status->setText("Real");
 
    }
}
 
void armarx::MMMPlayerWidget::on_previewButton_clicked()
{
    trajPlayer->setIsPreview(true);
    ARMARX_INFO << "speed spin box val: " <<  ui.speedSpinBox->value();
    setSpeed(ui.speedSpinBox->value());
    trajPlayer->resetTrajectoryPlayer(false);
    bool previewed = trajPlayer->startTrajectoryPlayer();
 
    if (previewed)
    {
        updateTimer->start(100);
        ui.initializeButton->setDisabled(previewed);
        ui.startButton->setDisabled(previewed);
        ui.previewButton->setDisabled(previewed);
        ui.motionName->setDisabled(previewed);
        ui.controlMode->setDisabled(previewed);
        ui.pauseButton->setEnabled(previewed);
        ui.pauseButton->setText("Pause");
        ui.btnSelectConfig->setEnabled(false);
        ui.speedSpinBox->setDisabled(previewed);
        ui.stopButton->setEnabled(previewed);
        ui.frameSlider->setDisabled(previewed);
        ui.frameSlider->blockSignals(true);
 
        ui.status->setText("Preview");
    }
}
 
void MMMPlayerWidget::on_pauseButton_clicked()
{
    bool paused = trajPlayer->pauseTrajectoryPlayer();
 
    if (paused)
    {
        ui.pauseButton->setText("Resume");
    }
    else
    {
        ui.pauseButton->setText("Pause");
    }
 
    ui.controlMode->setEnabled(paused);
    ui.frameSlider->setEnabled(paused);
    ui.speedSpinBox->setEnabled(paused);
    ui.frameSlider->blockSignals(!paused);
}
 
void armarx::MMMPlayerWidget::on_stopButton_clicked()
{
    bool stopped = trajPlayer->stopTrajectoryPlayer();
 
    if (stopped)
    {
        updateTimer->stop();
        ui.initializeButton->setEnabled(stopped);
        ui.startButton->setDisabled(stopped);
        ui.previewButton->setEnabled(stopped);
        ui.pauseButton->setText("Pause");
        ui.btnSelectConfig->setEnabled(true);
        ui.motionName->setEnabled(stopped);
        ui.controlMode->setEnabled(stopped);
        ui.speedSpinBox->setEnabled(stopped);
        ui.frameSlider->blockSignals(false);
        ui.pauseButton->setDisabled(stopped);
        ui.stopButton->setDisabled(stopped);
 
        ui.status->setText("");
    }
}
 
void MMMPlayerWidget::configFileSelected()
{
    configFile = (this->fileDialog2->selectedFiles()[0]).toStdString();
    ui.editConfigFile->setText(this->fileDialog2->selectedFiles()[0]);
 
    // load mmm motion
    MMMLoader->loadMMMFile(configFile, "");
    ui.frameSlider->setMaximum(100);
    setupJointList();
    setupMotionList();
 
    if (MMMLoader->isMotionLoaded())
    {
        loadTrajPlayer();
    }
}
 
void MMMPlayerWidget::setupMotionList()
{
    Ice::StringSeq motionNames = MMMLoader->getMotionNames();
    QObject::disconnect(ui.motionName, SIGNAL(currentIndexChanged(int)), this, SLOT(motionChanged(int)));
    ui.motionName->clear();
 
    for (size_t i = 0; i < motionNames.size(); ++i)
    {
        ui.motionName->addItem(QString::fromStdString(motionNames.at(i)));
    }
 
    if (motionNames.size() > 0)
    {
        ui.motionName->setCurrentIndex(0);
        connect(ui.motionName, SIGNAL(currentIndexChanged(int)), this, SLOT(motionChanged(int)));
    }
}
 
void MMMPlayerWidget::setupJointList()
{
    Ice::StringSeq jointNames = MMMLoader->getJointNames();
    jointList->blockSignals(true);
    jointList->clear();
 
    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        std::string jointName = jointNames.at(i);
        QListWidgetItem* joint = new QListWidgetItem(QString::fromStdString(jointName), jointList);
        joint->setCheckState(Qt::Checked);
    }
 
    jointList->blockSignals(false);
}
 
void MMMPlayerWidget::updateSlider()
{
    double currentTime = trajPlayer->getCurrentTime();
    double endTime = trajPlayer->getEndTime();
    double posPer = currentTime / endTime ;
 
    int sliderPos = posPer * 100;
 
    ui.frameSlider->blockSignals(true);
    ui.frameSlider->setSliderPosition(sliderPos);
    ui.frameSlider->blockSignals(false);
    ui.currentFrame->display(currentTime);
}
 
void MMMPlayerWidget::jointListChanged(QListWidgetItem* joint)
{
    bool jointState;
 
    if (joint->checkState() == Qt::Checked)
    {
        jointState = trajPlayer->setJointsInUse(joint->text().toStdString(), true);
    }
    else
    {
        jointState = trajPlayer->setJointsInUse(joint->text().toStdString(), false);
    }
 
    if (jointState)
    {
        joint->setCheckState(Qt::Checked);
    }
    else
    {
        joint->setCheckState(Qt::Unchecked);
    }
}
 
void MMMPlayerWidget::motionChanged(int)
{
    std::string motionName = ui.motionName->currentText().toStdString();
    ARMARX_INFO << "Choosing motion with name '" << motionName << "'";
    MMMLoader->setMotionData(motionName);
 
    if (MMMLoader->isMotionLoaded())
    {
        setupJointList();
        loadTrajPlayer();
    }
}
 
 
void MMMPlayerWidget::on_spinBoxFPS_valueChanged(double value)
{
    if (value <= 0)
    {
        return;
    }
 
}
 
 
void MMMPlayerWidget::on_loopPlayback_toggled(bool state)
{
    trajPlayer->setLoopPlayback(state);
    ui.loopPlayback->setChecked(state);
}
 
void MMMPlayerWidget::on_controlMode_changed(int controlMode)
{
    ui.controlMode->setCurrentIndex(controlMode);
 
    if (controlMode == POSITION_CONTROL)
    {
        trajPlayer->setIsVelocityControl(false);
    }
    else
    {
        trajPlayer->setIsVelocityControl(true);
    }
}
 
void armarx::MMMPlayerWidget::on_enableRobotPoseUnit_toggled(bool state)
{
    trajPlayer->enableRobotPoseUnit(state);
    ui.enableRobotPoseUnit->setChecked(state);
}