SimulatorViewerApp.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-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    ArmarXSimulation::application::SimulatorViewer
 * @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimulatorViewerApp.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
//SoQT
#include <Inventor/Qt/SoQt.h>
 
using namespace armarx;
 
// Redraw with 30Hz
#define MAIN_LOOP_SLEEP_MS 33
 
// print warnings if render time is low (ms)
#define MAX_RENDER_TIME_WARNING_MS 33
 
SimulatorViewerApp::SimulatorViewerApp()
    : Application(),
      exitApplication(false)
{
}
 
SimulatorViewerApp::~SimulatorViewerApp()
{
    ARMARX_VERBOSE_S << "destructor";
    //SoQt::done();
    qApplication->quit();
 
}
 
void SimulatorViewerApp::setup(
    const ManagedIceObjectRegistryInterfacePtr& registry,
    Ice::PropertiesPtr properties)
{
    // init SoDB and Qt, SoQt::init also creates an application object
    VirtualRobot::init("SimulatorViewerApp");
 
    mutex.reset(new CoinViewer::RecursiveMutex());
 
 
    // create the debugdrawer / entityDrawer component
    if (getProperty<bool>("UseDebugDrawer").getValue())
    {
        std::stringstream ddName;
        ddName << getName() << "_EntityDrawer";
        debugDrawer = Component::create<memoryx::EntityDrawerComponent>(properties, ddName.str());
        debugDrawer->setMutex(mutex);
        registry->addObject(debugDrawer);
    }
 
    std::stringstream svName;
    svName << getName() << "_PhysicsWorldVisualization";
    simVisu = Component::create<ArmarXPhysicsWorldVisualization>(properties, svName.str());
    //simVisu.reset(new ArmarXPhysicsWorldVisualization(getArmarXManager()));
    simVisu->setMutex(mutex);
    registry->addObject(simVisu);
 
 
    // create top-level widget
    std::stringstream swName;
    swName << getName() << "_SimulationWindow";
    mainWindow = Component::create<ArmarXSimulatorWindow>(properties, swName.str());
    mainWindow->setMutex(mutex);
    registry->addObject(mainWindow);
 
    //mainWindow.reset(new ArmarXSimulatorWindow(registry, simVisu, debugDrawer));
 
 
}
 
 
int SimulatorViewerApp::exec(const ArmarXManagerPtr& armarXManager)
{
 
    mainWindow->initialize(simVisu, debugDrawer);
    mainWindow->setCamPoseFromConfig();
    qApplication = QApplication::instance();
    qApplication->connect(mainWindow.get(), SIGNAL(closeRequest()), this, SLOT(closeRequest_sent()));
 
    //int timeoutMs = 30000;
    //if (!world->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectInitialized, timeoutMs))
    //    ARMARX_WARNING_S << "ManagedIceObject " << world->getName() << " was not initialized after "<< timeoutMs/1000 << " seconds" << std::endl;
 
    std::cout <<  "Started ArmarXSimulatorViewerApp" << std::endl;
 
    IceUtil::Time lastWarnTimeRender = IceUtil::Time::now();
    /*
        ARMARX_INFO_S << "Waiting for ManagedIceObject startup: " << physicsWorld->getName() << armarx::flush;
        int timeoutMs = 30000;
        ARMARX_INFO_S << "Waiting for ManagedIceObject startup: " << physicsWorld->getName() << ":ok" << armarx::flush;
        */
    // initialize visualization
 
    mainWindow->show();
 
 
#if 0
    memoryx::EntityDrawerInterfacePrx prxDD = armarXManager->getIceManager()->getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
    memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx = armarXManager->getIceManager()->getProxy<memoryx::PriorKnowledgeInterfacePrx>("PriorKnowledge");
 
    std::string objClassName = "vitaliscereal";
    memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
    memoryx::EntityBasePtr classesEntity = classesSegmentPrx->getEntityByName(objClassName);
    memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(classesEntity);
 
 
    Eigen::Matrix4f p;
    p.setIdentity();
    p(2, 3) = 1000.0f;
    PosePtr gp(new Pose(p));
    prxDD->setObjectVisu("debug", "vitalis", objectClass, gp);
 
    int color = 0;
#endif
    armarx::CycleUtil cycle(IceUtil::Time::secondsDouble(1.0 / getProperty<float>("UpdateRate").getValue()));
    // loop until ice quit or window closed
    while (!armarXManager->isShutdown() && !exitApplication)
    {
        try
        {
 
#if 0
            color = (color + 1) % 1000;
            DrawColor col;
            col.r = 1.0f;
            col.g = 0.0f;
            col.b = 0.0f;
            col.a = float(color) / 1000.0f;
 
            prxDD->updateObjectColor("debug", "vitalis", col);
#endif
            /*if (prxDD)
            {
                //ARMARX_INFO_S << "TESTING DEBUG DRAWER";
                // just a test
                Eigen::Matrix4f p;
                p.setIdentity();
                p(2,3) = 1000.0f;
                PosePtr gp(new Pose(p));
                prxDD->setPoseDebugLayerVisu("testPose",gp);
 
                armarx::Vector3Ptr p1(new armarx::Vector3());
                p1->x = 0;
                p1->y = 0;
                p1->z = 0;
                armarx::Vector3Ptr p2(new armarx::Vector3());
                p2->x = 1000.0f;
                p2->y = 1000.0f;
                p2->z = 1000.0f;
 
                armarx::DrawColor c;
                c.r = 1.0f;
                c.g = 0;
                c.b = 0;
                c.a = 1;
                prxDD->setLineDebugLayerVisu("test",p1,p2,10.0f,c);
            }*/
 
            // PAINT WINDOW
            IceUtil::Time startTime = IceUtil::Time::now();
            {
                {
                    auto startTime1 = IceUtil::Time::now();
                    auto l = mainWindow->getScopedLock();
                    IceUtil::Time duration1 = IceUtil::Time::now() - startTime1;
                    if (duration1.toMilliSecondsDouble() > 5)
                    {
                        ARMARX_VERBOSE << deactivateSpam(10) << " mainWindow->getScopedLock() took long:" << duration1.toMilliSecondsDouble();
                    }
                    simVisu->synchronizeVisualizationData();
                }
 
                // ensure engine is locked while we draw the content (there might be requests from ice to update the models)
                auto startTime1 = IceUtil::Time::now();
                // allow max 20ms for event processing
                qApplication->processEvents(QEventLoop::AllEvents);
                IceUtil::Time duration1 = IceUtil::Time::now() - startTime1;
                if (duration1.toMilliSecondsDouble() > 20)
                {
                    ARMARX_VERBOSE << deactivateSpam(10) << " qApplication->processEvents took long:" << duration1.toMilliSecondsDouble();
                }
            }
            IceUtil::Time duration = IceUtil::Time::now() - startTime;
            mainWindow->setDrawTimeMeasured((float)duration.toMilliSecondsDouble());
            ARMARX_VERBOSE << deactivateSpam(5) << "*** END paint, drawTimeMS:" << duration.toMilliSecondsDouble();
 
            if (duration.toMilliSecondsDouble() > MAX_RENDER_TIME_WARNING_MS)
            {
                IceUtil::Time lastWarnDuration = IceUtil::Time::now() - lastWarnTimeRender;
 
                if (lastWarnDuration.toSecondsDouble() > 4.0)
                {
                    lastWarnTimeRender = IceUtil::Time::now();
                    ARMARX_DEBUG << "*** Rendering slow !! Render time in ms:" << duration.toMilliSecondsDouble();
                }
            }
 
            cycle.waitForCycleDuration();
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    ARMARX_IMPORTANT_S << "Leaving main loop";
 
    //interruptCallback(0);// signal to the application it should terminate the ice connection
    armarXManager->shutdown();
 
    armarXManager->waitForShutdown();
 
    mainWindow = NULL;
    debugDrawer = NULL;
    simVisu = NULL;//.reset();
    qApplication->quit();
 
    return 0;
}
 
void SimulatorViewerApp::interruptCallback(int signal)
{
    ARMARX_IMPORTANT_S << "Interrupting ArmarXSimulatorViewerApp";
    exitApplication = true;
}
 
void SimulatorViewerApp::closeRequest_sent()
{
    ARMARX_INFO_S << "Stopping ArmarXSimulatorViewerApp";
    exitApplication = true;
}