ArmarXSimulatorWindow.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 "ArmarXSimulatorWindow.h"
 
// ArmarX
#include "ArmarXCore/core/Component.h"
#include "ArmarXCore/core/logging/LogSender.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
 
// Qt
#include <stdlib.h>
 
#include <filesystem>
 
#include <QFile>
#include <QFileDialog>
#include <QGLWidget>
#include <QImage>
#include <QInputDialog>
#include <QLabel>
#include <QListView>
#include <QMessageBox>
#include <QPushButton>
#include <QTimer>
#include <QToolButton>
#include <QtCore/QDirIterator>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
 
#include <Inventor/actions/SoBoxHighlightRenderAction.h>
#include <Inventor/nodes/SoCamera.h>
#include <Inventor/nodes/SoUnits.h>
#include <SimDynamics/DynamicsEngine/DynamicsRobot.h>
 
#define ARMARX_GUI_APPLICATION_NAME "ArmarXSimulator"
 
 
#define TIMER_MS 30.0f
 
using namespace armarx;
using namespace VirtualRobot;
using namespace SimDynamics;
 
ArmarXSimulatorWindow::ArmarXSimulatorWindow() :
    QMainWindow(NULL), viewer(nullptr), sceneSep(nullptr)
{
}
 
void
ArmarXSimulatorWindow::initialize(ArmarXPhysicsWorldVisualizationPtr physicsVisu,
                                  DebugDrawerComponentPtr debugDrawer)
{
    drawTimeMS = 0;
    guiWindowBaseName = ARMARX_GUI_APPLICATION_NAME;
    setWindowTitle(guiWindowBaseName);
    setTag(ARMARX_GUI_APPLICATION_NAME);
    ARMARX_INFO << " ************** ArmarXSimulatorWindow INIT ************** " << armarx::flush;
 
    setAttribute(Qt::WA_QuitOnClose);
 
    this->physicsVisu = physicsVisu;
    this->debugDrawer = debugDrawer;
 
    sceneSep = new SoSeparator;
    sceneSep->ref();
 
    if (physicsVisu && physicsVisu->getVisualization())
    {
        sceneSep->addChild(physicsVisu->getVisualization());
    }
 
    if (debugDrawer && debugDrawer->getVisualization())
    {
        sceneSep->addChild(debugDrawer->getVisualization());
    }
 
    setupUI();
}
 
void
ArmarXSimulatorWindow::onInitComponent()
{
}
 
void
ArmarXSimulatorWindow::setCamPoseFromConfig()
{
    float x = getProperty<float>("Camera.x").getValue();
    float y = getProperty<float>("Camera.y").getValue();
    float z = getProperty<float>("Camera.z").getValue();
    float ro = getProperty<float>("Camera.roll").getValue();
    float pi = getProperty<float>("Camera.pitch").getValue();
    float ya = getProperty<float>("Camera.yaw").getValue();
    setCamPose(x, y, z, ro, pi, ya);
}
 
void
ArmarXSimulatorWindow::onConnectComponent()
{
    setCamPoseFromConfig();
 
    bool showBase = getProperty<bool>("ShowBaseCoordSystem").getValue();
    showBaseCoordSystem(showBase);
 
    int alias = getProperty<int>("AntiAliasingRenderPasses").getValue();
    setAntiAliasing(alias);
 
    saveVideo = getProperty<bool>("SaveVideo").getValue();
    tmpDir = getProperty<std::string>("TempDir").getValue();
}
 
void
ArmarXSimulatorWindow::onDisconnectComponent()
{
}
 
void
ArmarXSimulatorWindow::onExitComponent()
{
}
 
void
ArmarXSimulatorWindow::setupUI()
{
    ui.setupUi(this);
 
    viewer = new CoinViewer(ui.frameViewer,
                            "ArmarXSimulator",
                            TRUE,
                            SoQtExaminerViewer::BUILD_POPUP,
                            SoQtExaminerViewer::BROWSER);
    viewer->setMutex(mutex);
 
    // setup
    viewer->setBackgroundColor(SbColor{getProperty<float>("Background.r").getValue(),
                                       getProperty<float>("Background.g").getValue(),
                                       getProperty<float>("Background.b").getValue()});
 
    int numPasses = getProperty<int>("AntiAliasingRenderPasses").getValue();
    viewer->setAccumulationBuffer(numPasses > 0);
    //viewer->setAntialiasing(false, 1);
 
    viewer->setGLRenderAction(new SoBoxHighlightRenderAction);
    viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
    viewer->setFeedbackVisibility(true);
    viewer->setSceneGraph(sceneSep);
    viewer->viewAll();
    // really needed?
    //viewer->setAccumulationBuffer(numPasses > 0);
 
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    SoTimerSensor* timer = new SoTimerSensor(timerCB, this);
    timer->setInterval(SbTime(TIMER_MS / 1000.0f));
    sensor_mgr->insertTimerSensor(timer);
}
 
ArmarXSimulatorWindow::~ArmarXSimulatorWindow()
{
    if (sceneSep)
    {
        sceneSep->unref();
    }
    delete viewer;
    ARMARX_INFO << "~ArmarXSimulatorWindow() ";
}
 
void
ArmarXSimulatorWindow::closeEvent(QCloseEvent* event)
{
    emit closeRequest();
    event->accept();
}
 
void
ArmarXSimulatorWindow::timerCB(void* data, SoSensor* /*sensor*/)
{
    ArmarXSimulatorWindow* simWindow = static_cast<ArmarXSimulatorWindow*>(data);
 
    //simWindow->redraw();
    simWindow->saveScreenshot();
}
 
void
ArmarXSimulatorWindow::saveScreenshot()
{
    if (!saveVideo)
    {
        return;
    }
 
    static long counter = 0;
    clock_t start = clock();
 
    //framefile.sprintf("renderFrame_%06d.png", counter);
    std::stringstream ss;
    ss << tmpDir << "/armarx-render-frame-" << counter << ".bmp";
    std::string fn = ss.str();
    counter++;
    //redraw();
 
    viewer->getSceneManager()->render();
    //viewer->getSceneManager()->scheduleRedraw();
 
    QGLWidget* w = (QGLWidget*)viewer->getGLWidget();
 
    QImage i = w->grabFrameBuffer();
    QString fnQ(fn.c_str());
    bool res = i.save(fnQ, "BMP");
    if (!res)
    {
        ARMARX_WARNING << deactivateSpam(5) << "Could not write screenshot to file " << fn;
    }
    else
    {
        clock_t end = clock();
        float timeMS = (float)(end - start) / (float)CLOCKS_PER_SEC * 1000.0f;
        ARMARX_INFO << deactivateSpam(5) << "Wrote image " << fn << " in " << timeMS << " ms";
    }
}
 
void
ArmarXSimulatorWindow::setupAntiAliasing(int numPasses)
{
    auto l = getScopedLock();
 
    if (viewer)
    {
        viewer->setAccumulationBuffer(numPasses > 0);
        viewer->setAntialiasing((numPasses > 0), numPasses);
    }
}
 
void
ArmarXSimulatorWindow::slotShowBaseCoord(bool show, float scale)
{
    if (physicsVisu)
    {
        physicsVisu->showBaseCoord(show, scale);
    }
}
 
void
ArmarXSimulatorWindow::slotEnableDebugDrawLayer(bool show)
{
    auto l = getScopedLock();
 
    if (show && sceneSep && debugDrawer && debugDrawer->getVisualization() &&
        sceneSep->findChild(debugDrawer->getVisualization()) < 0)
    {
        sceneSep->addChild(debugDrawer->getVisualization());
    }
 
    if (!show && sceneSep && debugDrawer && debugDrawer->getVisualization() &&
        sceneSep->findChild(debugDrawer->getVisualization()) >= 0)
    {
        sceneSep->removeChild(debugDrawer->getVisualization());
    }
}
 
void
ArmarXSimulatorWindow::viewAll()
{
    auto l = getScopedLock();
    ARMARX_DEBUG << "viewall" << flush;
 
    if (!viewer || !physicsVisu)
    {
        return;
    }
 
    SoCamera* cam = viewer->getCamera();
    SoSeparator* sceneViewableSep = physicsVisu->getViewableScene();
 
    if (cam && sceneViewableSep)
    {
        cam->viewAll(sceneViewableSep, viewer->getViewportRegion());
    }
}
 
Eigen::Matrix4f
ArmarXSimulatorWindow::getCamPose()
{
    auto l = getScopedLock();
    Eigen::Matrix4f camPose = Eigen::Matrix4f::Identity();
 
    if (viewer && viewer->getCamera())
    {
        SbVec3f position = viewer->getCamera()->position.getValue();
        SbRotation orientation = viewer->getCamera()->orientation.getValue();
        SbVec3f rotAxis;
        float angle;
        orientation.getValue(rotAxis, angle);
        Eigen::Vector3f axE;
        axE(0) = rotAxis[0];
        axE(1) = rotAxis[1];
        axE(2) = rotAxis[2];
        camPose = MathTools::axisangle2eigen4f(axE, angle);
        camPose(0, 3) = position[0];
        camPose(1, 3) = position[1];
        camPose(2, 3) = position[2];
    }
 
    return camPose;
}
 
void
ArmarXSimulatorWindow::setCamPose(float x, float y, float z, float roll, float pitch, float yaw)
{
    ARMARX_VERBOSE << "setting cam pose " << VAROUT(x) << VAROUT(y) << VAROUT(z) << VAROUT(roll)
                   << VAROUT(pitch) << VAROUT(yaw);
    auto l = getScopedLock();
 
    if (viewer && viewer->getCamera())
    {
        SbVec3f position(x * 0.001f, y * 0.001f, z * 0.001f);
        SbRotation orientation;
        Eigen::Matrix4f m;
        MathTools::rpy2eigen4f(roll, pitch, yaw, m);
        float ang;
        Eigen::Vector3f ax;
        MathTools::eigen4f2axisangle(m, ax, ang);
        SbVec3f ax2(ax(0), ax(1), ax(2));
        orientation.setValue(ax2, ang);
        viewer->getCamera()->position.setValue(position);
        viewer->getCamera()->orientation.setValue(orientation);
    }
}
 
CoinViewer*
ArmarXSimulatorWindow::getViewer()
{
    return viewer;
}
 
SoNode*
ArmarXSimulatorWindow::getScene()
{
    return sceneSep;
}
 
void
ArmarXSimulatorWindow::setMutex(CoinViewer::RecursiveMutexPtr const& m)
{
    mutex = m;
 
    if (viewer)
    {
        viewer->setMutex(m);
    }
}
 
CoinViewer::RecursiveMutexLockPtr
ArmarXSimulatorWindow::getScopedLock()
{
    CoinViewer::RecursiveMutexLockPtr l(new CoinViewer::RecursiveMutexLock(*mutex));
    return l;
}
 
void
ArmarXSimulatorWindow::enableSimulatorWindow(bool show, const Ice::Current& c)
{
    {
        auto l = getScopedLock();
        info.windowEnabled = show;
    }
 
    if (show)
    {
        ARMARX_INFO << "Showing simulator window";
        QMetaObject::invokeMethod(
            this,
            "show"); //, Q_ARG(float, currentPlatformPositionX), Q_ARG(float, currentPlatformPositionY), Q_ARG(float, currentPlatformRotation));
 
        //simWindow->show();
    }
    else
    {
        ARMARX_INFO << "Hiding simulator window";
        QMetaObject::invokeMethod(this, "hide");
        //simWindow->hide();
    }
}
 
void
ArmarXSimulatorWindow::showBaseCoordSystem(bool show, float scale, const Ice::Current& c)
{
    QMetaObject::invokeMethod(
        this, "slotShowBaseCoord", Qt::QueuedConnection, Q_ARG(bool, show), Q_ARG(float, scale));
}
 
/*
void ArmarXSimulatorWindow::showContacts(bool show, const Ice::Current &c)
{
    QMetaObject::invokeMethod(this, "slotShowContacts", Qt::QueuedConnection, Q_ARG(bool, show));
}*/
 
armarx::SimulatorViewerInformation
ArmarXSimulatorWindow::getSimulatorInformation(const Ice::Current& c)
{
    auto l = getScopedLock();
 
    // update data
    if (physicsVisu)
    {
        info.comTimeMS = physicsVisu->getSyncVisuTime();
    }
    else
    {
        info.comTimeMS = 0;
    }
 
    info.drawTimeMS = drawTimeMS;
 
    return info;
}
 
void
ArmarXSimulatorWindow::setDrawTimeMeasured(float ms)
{
    drawTimeMS = ms;
}
 
void
ArmarXSimulatorWindow::setAntiAliasing(int steps, const Ice::Current& c)
{
    if (steps < 0)
    {
        steps = 0;
    }
 
 
    if (!QMetaObject::invokeMethod(
            this, "setupAntiAliasing", Qt::QueuedConnection, Q_ARG(int, steps)))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
PoseBasePtr
ArmarXSimulatorWindow::getCameraPose(const Ice::Current& c)
{
    // maybe we should invoke a qt-thread safe method, but a lock (used in getCamPose) seems to be fine for now...
    Eigen::Matrix4f camPose = getCamPose();
    PosePtr p(new Pose(camPose));
    return p;
}
 
void
ArmarXSimulatorWindow::showDebugDrawLayer(bool show, const Ice::Current& c)
{
    if (!QMetaObject::invokeMethod(
            this, "slotEnableDebugDrawLayer", Qt::QueuedConnection, Q_ARG(bool, show)))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
void
ArmarXSimulatorWindow::slotClearDebugDrawLayer()
{
    ARMARX_VERBOSE << "Clearing debug layer...";
 
    if (debugDrawer)
    {
        debugDrawer->clearLayer("debug");
    }
}
 
void
ArmarXSimulatorWindow::clearDebugDrawLayer(const Ice::Current& c)
{
    if (!QMetaObject::invokeMethod(this, "slotClearDebugDrawLayer", Qt::QueuedConnection))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
void
ArmarXSimulatorWindow::clearLayer(const std::string& layerName, const ::Ice::Current&)
{
    QString s(layerName.c_str());
 
    if (!QMetaObject::invokeMethod(this, "slotClearLayer", Qt::QueuedConnection, Q_ARG(QString, s)))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
void
ArmarXSimulatorWindow::removeLayer(const std::string& layerName, const ::Ice::Current&)
{
    QString s(layerName.c_str());
 
    if (!QMetaObject::invokeMethod(
            this, "slotRemoveLayer", Qt::QueuedConnection, Q_ARG(QString, s)))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
void
ArmarXSimulatorWindow::enableLayerVisu(const std::string& layerName,
                                       bool visible,
                                       const ::Ice::Current&)
{
    QString s(layerName.c_str());
 
    if (!QMetaObject::invokeMethod(this,
                                   "slotEnableLayerVisu",
                                   Qt::QueuedConnection,
                                   Q_ARG(QString, s),
                                   Q_ARG(bool, visible)))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
void
ArmarXSimulatorWindow::slotClearLayer(const QString layerName)
{
    if (debugDrawer)
    {
        debugDrawer->clearLayer(layerName.toStdString());
    }
}
 
void
ArmarXSimulatorWindow::slotRemoveLayer(const QString layerName)
{
    if (debugDrawer)
    {
        debugDrawer->removeLayer(layerName.toStdString());
    }
}
 
void
ArmarXSimulatorWindow::slotEnableLayerVisu(const QString layerName, bool visible)
{
    if (debugDrawer)
    {
        debugDrawer->enableLayerVisu(layerName.toStdString(), visible);
    }
}
 
::armarx::LayerInformationSequence
ArmarXSimulatorWindow::layerInformation(const ::Ice::Current&)
{
    if (debugDrawer)
    {
        return debugDrawer->layerInformation();
    }
 
    return ::armarx::LayerInformationSequence{};
}
 
void
ArmarXSimulatorWindow::setTempPath(const std::string& p, const Ice::Current&)
{
    tmpDir = p;
}
 
void
ArmarXSimulatorWindow::saveScreenshots(bool enable, const Ice::Current&)
{
    saveVideo = enable;
}
 
void
ArmarXSimulatorWindow::selectVisuType(bool fullModel, const Ice::Current& c)
{
    if (!QMetaObject::invokeMethod(
            this, "slotSelectVisuType", Qt::QueuedConnection, Q_ARG(bool, fullModel)))
    {
        ARMARX_VERBOSE << "Failed to invoke method";
    }
}
 
void
ArmarXSimulatorWindow::slotSelectVisuType(bool fullModel)
{
    if (physicsVisu)
    {
        physicsVisu->enableVisuType(fullModel);
    }
}