ArmarXPhysicsWorldVisualization.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 "ArmarXPhysicsWorldVisualization.h"
 
#include <VirtualRobot/ManipulationObject.h> // IWYU prgma: keep
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNodeActuator.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <Inventor/nodes/SoUnits.h>
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
 
using namespace VirtualRobot;
using namespace memoryx;
 
namespace armarx
{
 
    ArmarXPhysicsWorldVisualization::ArmarXPhysicsWorldVisualization()
    {
        //enableContactVisu = false;
 
        // ArmarXPhysicsWorldVisualization_run__register_component_executable__test currently broken here: likely internal coin issue
        coinVisu = new SoSeparator;
        coinVisu->ref();
 
        sceneViewableSep = new SoSeparator;
        coinVisu->addChild(sceneViewableSep);
 
        dynamicsVisu = new SoSeparator;
        dynamicsVisu->ref();
        sceneViewableSep->addChild(dynamicsVisu);
        baseCoordVisu = new SoSeparator;
        baseCoordVisu->ref();
 
        // floor is not part of the viewable visu (allows better viewAll handling)
        floorVisu = new SoSeparator;
        this->lastSceneData.floor = false;
        coinVisu->addChild(floorVisu);
 
        modelType = VirtualRobot::SceneObject::Full;
 
        // initially set mutex
        mutex.reset(new std::recursive_mutex());
        mutexData.reset(new std::recursive_mutex());
    }
 
    ArmarXPhysicsWorldVisualization::~ArmarXPhysicsWorldVisualization()
    {
        ARMARX_INFO << "Destroying PhysicsWorld";
    }
 
    std::string
    ArmarXPhysicsWorldVisualization::GetDefaultName()
    {
        return "ArmarXPhysicsWorldVisualization";
    }
 
    template <typename T>
    static void
    safeUnref(T*& p)
    {
        if (p)
        {
            p->unref();
            p = nullptr;
        }
    }
 
    void
    ArmarXPhysicsWorldVisualization::releaseResources()
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
 
        addedVisualizations.clear();
        addedRobotVisualizations.clear();
 
        safeUnref(coinVisu);
        safeUnref(dynamicsVisu);
        safeUnref(baseCoordVisu);
    }
 
    void
    ArmarXPhysicsWorldVisualization::onInitComponent()
    {
        ARMARX_TRACE;
        std::string top = getProperty<std::string>("ReportVisuTopicName").getValue();
        ARMARX_VERBOSE << "Using topic " << top;
        usingTopic(top);
    }
 
    void
    ArmarXPhysicsWorldVisualization::onConnectComponent()
    {
    }
 
    void
    ArmarXPhysicsWorldVisualization::onDisconnectComponent()
    {
    }
 
    void
    ArmarXPhysicsWorldVisualization::onExitComponent()
    {
    }
 
    void
    ArmarXPhysicsWorldVisualization::reportSceneUpdated(const SceneVisuData& actualData,
                                                        const Ice::Current&)
    {
        ARMARX_TRACE;
        auto l = getScopedDataLock();
        this->currentSceneData = actualData;
    }
 
    SoSeparator*
    ArmarXPhysicsWorldVisualization::getVisualization()
    {
        return coinVisu;
    }
 
    SoSeparator*
    ArmarXPhysicsWorldVisualization::getViewableScene()
    {
        return sceneViewableSep;
    }
 
    SoNode*
    ArmarXPhysicsWorldVisualization::buildVisu(
        SceneObjectPtr o,
        VirtualRobot::SceneObject::VisualizationType visuType)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        SoNode* n = CoinVisualizationFactory::getCoinVisualization(o, visuType);
        n->ref();
        return n;
    }
 
    SoNode*
    ArmarXPhysicsWorldVisualization::buildVisu(
        RobotPtr ro,
        VirtualRobot::SceneObject::VisualizationType visuType)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        ARMARX_DEBUG << "buildVisu type: " << visuType;
        std::vector<RobotNodePtr> collectedRobotNodes = ro->getRobotNodes();
        ARMARX_DEBUG << "Robot visu with " << collectedRobotNodes.size() << " nodes";
        SoSeparator* n = new SoSeparator();
        n->ref();
        for (const RobotNodePtr& node : collectedRobotNodes)
        {
            ARMARX_TRACE;
            VisualizationNodePtr visu = node->getVisualization(visuType);
            auto coinVisualizationNode = std::dynamic_pointer_cast<CoinVisualizationNode>(visu);
            if (coinVisualizationNode)
            {
                ARMARX_TRACE;
                if (coinVisualizationNode->getCoinVisualization())
                {
                    ARMARX_TRACE;
                    ARMARX_DEBUG << "ADDING VISU " << node->getName();
                    SoNode* no = coinVisualizationNode->getCoinVisualization();
                    SoSeparator* sep = dynamic_cast<SoSeparator*>(no);
                    if (sep)
                    {
                        ARMARX_DEBUG << "ADDING SEP count " << sep->getNumChildren();
                    }
 
                    n->addChild(no);
                }
                else
                {
                    ARMARX_DEBUG << "Node " << node->getName() << " doesn't have a coin visu model";
                }
            }
            else
            {
                ARMARX_DEBUG << "Node " << node->getName() << " doesn't have a visu model";
            }
        }
 
        ARMARX_DEBUG << "buildVisu end";
        return n;
    }
 
    void
    ArmarXPhysicsWorldVisualization::addVisualization(
        RobotPtr ro,
        VirtualRobot::SceneObject::VisualizationType visuType)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
 
        VR_ASSERT(ro);
        removeVisualization(ro);
 
        if (SoNode* n = buildVisu(ro, visuType))
        {
            ARMARX_TRACE;
            dynamicsVisu->addChild(n);
            addedRobotVisualizations[ro] = n;
            n->unref();
        }
 
        // we have our own mutex protection
        ro->setThreadsafe(false);
    }
 
    void
    ArmarXPhysicsWorldVisualization::addFloorVisualization(Eigen::Vector3f floorPos,
                                                           Eigen::Vector3f floorUp,
                                                           float floorExtendMM,
                                                           std::string floorTexture)
    {
        ARMARX_TRACE;
        ARMARX_INFO << "Adding floor visu, texture file:" << floorTexture;
        auto l = getScopedLock();
        floorVisu->removeAllChildren();
        SoNode* n = (SoNode*)CoinVisualizationFactory::CreatePlaneVisualization(
            floorPos,
            floorUp,
            floorExtendMM,
            true,
            CoinVisualizationFactory::Color::Gray(),
            floorTexture);
        if (n)
        {
            ARMARX_TRACE;
            floorVisu->addChild(n);
        }
    }
 
    void
    ArmarXPhysicsWorldVisualization::removeFloorVisualization()
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        floorVisu->removeAllChildren();
    }
 
    void
    ArmarXPhysicsWorldVisualization::showBaseCoord(bool enable, float scale)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
 
        if (enable && sceneViewableSep->findChild(baseCoordVisu) < 0)
        {
            ARMARX_TRACE;
            baseCoordVisu->removeAllChildren();
            std::string str("Root");
            baseCoordVisu->addChild(
                VirtualRobot::CoinVisualizationFactory::CreateCoordSystemVisualization(scale,
                                                                                       &str));
            sceneViewableSep->addChild(baseCoordVisu);
        }
 
        if (!enable && sceneViewableSep->findChild(baseCoordVisu) >= 0)
        {
            ARMARX_TRACE;
            sceneViewableSep->removeChild(baseCoordVisu);
        }
    }
 
    void
    ArmarXPhysicsWorldVisualization::addVisualization(
        SceneObjectPtr so,
        VirtualRobot::SceneObject::VisualizationType visuType)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(so);
 
        auto l = getScopedLock();
        removeVisualization(so);
        SoNode* n = buildVisu(so, visuType);
        ARMARX_DEBUG << "addVisualization " << so->getName() << " Ref: " << n->getRefCount();
        if (n)
        {
            ARMARX_TRACE;
            dynamicsVisu->addChild(n);
            addedVisualizations[so] = n;
            n->unref();
            ARMARX_DEBUG << "addVisualization " << so->getName() << " Ref: " << n->getRefCount();
        }
    }
 
    void
    ArmarXPhysicsWorldVisualization::removeVisualization(SceneObjectPtr so)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        ARMARX_DEBUG << "removeVisualization" << flush;
        VR_ASSERT(so);
 
        if (addedVisualizations.find(so) != addedVisualizations.end())
        {
            ARMARX_TRACE;
            SoNode* node = addedVisualizations[so];
            ARMARX_DEBUG << "RemoveVisualization: " << so->getName()
                         << ", Ref: " << node->getRefCount();
            addedVisualizations.erase(so);
            dynamicsVisu->removeChild(node);
        }
    }
 
    void
    ArmarXPhysicsWorldVisualization::removeObjectVisualization(const std::string& name)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        ARMARX_DEBUG << "removeVisualization";
 
        // removeVisualizations erases elements from addedVisualizations so we have to make a copy
        auto copiedVisus = addedVisualizations;
        for (auto& o : copiedVisus)
        {
            ARMARX_TRACE;
            VirtualRobot::SceneObjectPtr const& so = o.first;
            if (so->getName() == name)
            {
                ARMARX_TRACE;
                removeVisualization(so);
                return;
            }
        }
 
        ARMARX_ERROR << "No object with name " << name << " found";
    }
 
    void
    ArmarXPhysicsWorldVisualization::removeRobotVisualization(const std::string& name)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        ARMARX_DEBUG << "remove robot visualization:" << name;
        VirtualRobot::RobotPtr so;
 
        for (auto& r : addedRobotVisualizations)
        {
            ARMARX_TRACE;
            if (r.first->getName() == name)
            {
                ARMARX_TRACE;
                removeVisualization(r.first);
                return;
            }
        }
 
        ARMARX_ERROR << "No robot with name " << name << " found";
    }
 
    void
    ArmarXPhysicsWorldVisualization::removeVisualization(RobotPtr r)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        ARMARX_DEBUG << "removeVisualization robot";
        VR_ASSERT(r);
 
        if (addedRobotVisualizations.find(r) != addedRobotVisualizations.end())
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "found added robot, removing " << r->getName();
            SoNode* node = addedRobotVisualizations[r];
            dynamicsVisu->removeChild(node);
            addedRobotVisualizations.erase(r);
        }
        ARMARX_DEBUG << "removeVisualization robot end";
    }
 
    void
    ArmarXPhysicsWorldVisualization::enableVisuType(bool fullModel)
    {
        ARMARX_TRACE;
        auto l = getScopedLock();
        ARMARX_DEBUG << "enableVisuType" << flush;
 
        if (fullModel && modelType == VirtualRobot::SceneObject::Full)
        {
            return;
        }
 
        if (!fullModel && modelType == VirtualRobot::SceneObject::Collision)
        {
            return;
        }
        ARMARX_TRACE;
 
        if (fullModel)
        {
            modelType = VirtualRobot::SceneObject::Full;
        }
        else
        {
            modelType = VirtualRobot::SceneObject::Collision;
        }
 
        if (fullModel)
        {
            ARMARX_INFO << "Switching to visuType full:";
        }
        else
        {
            ARMARX_INFO << "Switching to visuType colModel:";
        }
 
        dynamicsVisu->removeAllChildren();
 
        // rebuild robots
        for (const auto& pair : addedRobotVisualizations)
        {
            ARMARX_TRACE;
            const RobotPtr& ro = pair.first;
            SoNode* n = buildVisu(ro, modelType);
 
            if (n)
            {
                dynamicsVisu->addChild(n);
                addedRobotVisualizations[ro] = n;
                n->unref();
            }
            else
            {
                ARMARX_DEBUG << "Robot " << ro->getName() << " has no visualization for model type "
                             << modelType;
            }
        }
        ARMARX_TRACE;
 
        // rebuild objects
        std::map<VirtualRobot::SceneObjectPtr, SoNode*>::iterator it2 = addedVisualizations.begin();
 
        for (; it2 != addedVisualizations.end(); it2++)
        {
            ARMARX_TRACE;
            SceneObjectPtr so = it2->first;
            SoNode* n = buildVisu(so, modelType);
 
            if (n)
            {
                dynamicsVisu->addChild(n);
                addedVisualizations[so] = n;
                n->unref();
            }
        }
    }
 
    void
    ArmarXPhysicsWorldVisualization::setMutex(std::shared_ptr<std::recursive_mutex> const& m)
    {
        mutex = m;
    }
 
    auto
    ArmarXPhysicsWorldVisualization::getScopedLock() -> RecursiveLockPtr
    {
        RecursiveLockPtr l(new std::unique_lock(*mutex));
        return l;
    }
 
    auto
    ArmarXPhysicsWorldVisualization::getScopedDataLock() -> RecursiveLockPtr
    {
        RecursiveLockPtr l(new std::unique_lock(*mutexData));
        return l;
    }
 
    /*
    void ArmarXPhysicsWorldVisualization::enableContactVisu(bool enable)
    {
        contactVisuEnabled = enable;
    }*/
 
 
    bool
    ArmarXPhysicsWorldVisualization::synchronizeSceneObjectPoses(NamePoseMap& objMap,
                                                                 NameValueMap& jvMap,
                                                                 SceneObjectPtr currentObjVisu)
    {
        ARMARX_TRACE;
        if (!currentObjVisu)
        {
            return false;
        }
 
#if 0
 
        // some sanity checks:
        if (objMap.find(currentObjVisu->getName()) == objMap.end())
        {
            ARMARX_ERROR << "No object with name " << currentObjVisu->getName() << "in objMap";
        }
        else
        {
            ARMARX_IMPORTANT << "object " << currentObjVisu->getName() << ":\n" << objMap[currentObjVisu->getName()] ;
        }
 
#endif
 
        if (objMap.find(currentObjVisu->getName()) != objMap.end())
        {
            ARMARX_TRACE;
            VirtualRobot::RobotNodePtr rnv =
                std::dynamic_pointer_cast<VirtualRobot::RobotNode>(currentObjVisu);
            PosePtr p = PosePtr::dynamicCast(objMap[currentObjVisu->getName()]);
            Eigen::Matrix4f gp = p->toEigen();
 
            if (rnv)
            {
                ARMARX_TRACE;
                // we can update the joint value via an RobotNodeActuator
                RobotNodeActuatorPtr rna(new RobotNodeActuator(rnv));
                rna->updateVisualizationPose(gp, jvMap[currentObjVisu->getName()], false);
            }
            else
            {
                ARMARX_TRACE;
                currentObjVisu->setGlobalPoseNoChecks(gp);
            }
        }
        ARMARX_TRACE;
        std::vector<SceneObjectPtr> childrenV = currentObjVisu->getChildren();
 
        for (const auto& i : childrenV)
        {
            ARMARX_TRACE;
            if (!synchronizeSceneObjectPoses(objMap, jvMap, i))
            {
                return false;
            }
        }
 
        return true;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::synchronizeFloor()
    {
        ARMARX_TRACE;
        if (lastSceneData.floor == currentSceneData.floor &&
            lastSceneData.floorTextureFile == currentSceneData.floorTextureFile)
        {
            return true;
        }
 
        if (currentSceneData.floor)
        {
            ARMARX_TRACE;
            VirtualRobot::MathTools::Plane p = VirtualRobot::MathTools::getFloorPlane();
            addFloorVisualization(p.p, p.n, 50000.0f, currentSceneData.floorTextureFile);
        }
        else
        {
            removeFloorVisualization();
        }
 
        return true;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::synchronizeRobots()
    {
        ARMARX_TRACE;
        for (auto& robotDef : currentSceneData.robots)
        {
            ARMARX_TRACE;
            if (!robotDef.updated)
            {
                continue;
            }
 
            // check if we need to load the robot
            bool robotLoaded = false;
 
            for (auto& currentRob : lastSceneData.robots)
            {
                ARMARX_TRACE;
                if (robotDef.name == currentRob.name)
                {
                    robotLoaded = true;
                    break;
                }
            }
 
            if (!robotLoaded)
            {
                ARMARX_TRACE;
                ARMARX_INFO << deactivateSpam() << "New robot:" << robotDef.name;
 
                if (!loadRobot(robotDef.name,
                               robotDef.robotFile,
                               robotDef.project,
                               robotDef.scaling,
                               robotDef.colModel))
                {
                    ARMARX_ERROR << deactivateSpam() << "Loading robot failed";
                    continue;
                }
            }
 
            ARMARX_TRACE;
            VirtualRobot::RobotPtr rob = getRobot(robotDef.name);
 
            if (!rob)
            {
                ARMARX_ERROR << "internal error, no robot with name " << robotDef.name;
 
                for (auto& r : addedRobotVisualizations)
                {
                    ARMARX_ERROR << "Available Robot:" << r.first->getName();
                }
 
                continue;
            }
            ARMARX_TRACE;
 
            VirtualRobot::SceneObjectPtr currentObjVisu = rob->getRootNode();
 
            if (!synchronizeSceneObjectPoses(
                    robotDef.robotNodePoses, robotDef.jointValues, currentObjVisu))
            {
                ARMARX_ERROR << deactivateSpam() << "Failed to synchronize objects...";
                continue;
            }
            ARMARX_TRACE;
 
            // Run forward kinematics, including propagating joint values.
            const bool updateChildren = true;
            currentObjVisu->updatePose(updateChildren);
 
            // never use this data again
            robotDef.updated = false;
        }
        ARMARX_TRACE;
 
        // check if a robot is removed
        for (auto& currentRob : lastSceneData.robots)
        {
            ARMARX_TRACE;
            bool robFound = false;
 
            for (auto& robotDef : currentSceneData.robots)
            {
                if (robotDef.name == currentRob.name)
                {
                    robFound = true;
                    break;
                }
            }
 
            if (!robFound)
            {
                ARMARX_TRACE;
                ARMARX_INFO << "Removing robot:" << currentRob.name;
                removeRobotVisualization(currentRob.name);
            }
        }
 
        return true;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::synchronizeObjects()
    {
        ARMARX_TRACE;
        for (auto& objectDef : currentSceneData.objects)
        {
            ARMARX_TRACE;
            if (!objectDef.updated)
            {
                continue;
            }
 
            // check if we need to load the robot
            bool objectLoaded = false;
 
            for (auto& currentObj : lastSceneData.objects)
            {
                if (objectDef.name == currentObj.name && getObject(objectDef.name))
                {
                    objectLoaded = true;
                    break;
                }
            }
            ARMARX_TRACE;
 
            if (!objectLoaded)
            {
                ARMARX_INFO << deactivateSpam() << "New Object:" << objectDef.name;
 
                if (!loadObject(objectDef))
                {
                    ARMARX_ERROR << deactivateSpam(5, objectDef.name) << "Loading object "
                                 << objectDef.name << " failed"
                                 << "\n " << VAROUT(objectDef.name) << "\n "
                                 << VAROUT(objectDef.objectClassName) << "\n "
                                 << VAROUT(objectDef.filename) << "\n "
                                 << VAROUT(objectDef.project);
                    continue;
                }
            }
            ARMARX_TRACE;
 
            VirtualRobot::SceneObjectPtr ob = getObject(objectDef.name);
            if (!ob)
            {
                ARMARX_ERROR << "internal error, no object for " << objectDef.name;
                continue;
            }
 
            NameValueMap tmpMap;
 
            if (!synchronizeSceneObjectPoses(objectDef.objectPoses, tmpMap, ob))
            {
                ARMARX_ERROR << deactivateSpam() << "Failed to synchronize objects...";
                continue;
            }
            ARMARX_TRACE;
 
            // never use this data again
            objectDef.updated = false;
        }
        ARMARX_TRACE;
 
        // check if a object is removed
        for (auto& currentObj : lastSceneData.objects)
        {
            ARMARX_TRACE;
            bool objFound = false;
 
            for (auto& objectDef : currentSceneData.objects)
            {
                if (objectDef.name == currentObj.name)
                {
                    objFound = true;
                    break;
                }
            }
            ARMARX_TRACE;
 
            if (!objFound)
            {
                ARMARX_INFO << "Removing object:" << currentObj.name;
                removeObjectVisualization(currentObj.name);
            }
        }
 
        return true;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::synchronizeVisualizationData()
    {
        ARMARX_TRACE;
        IceUtil::Time startTime = IceUtil::Time::now();
 
        auto lockVisu = getScopedLock();
        auto lockData = getScopedDataLock();
 
        IceUtil::Time durationlock = IceUtil::Time::now() - startTime;
 
        if (durationlock.toMilliSecondsDouble() > 10)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "Copy and Lock took long: " << durationlock.toMilliSecondsDouble()
                        << " ms";
        }
        ARMARX_TRACE;
 
 
        // check for updates
        auto startTime1 = IceUtil::Time::now();
 
        synchronizeFloor();
 
        IceUtil::Time duration1 = IceUtil::Time::now() - startTime1;
        if (duration1.toMilliSecondsDouble() > 10)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << " floorSync took long: " << duration1.toMilliSecondsDouble() << " ms";
        }
        ARMARX_TRACE;
 
        auto startTime2 = IceUtil::Time::now();
 
        synchronizeRobots();
 
        IceUtil::Time duration2 = IceUtil::Time::now() - startTime2;
        if (duration2.toMilliSecondsDouble() > 10)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << " RobotSync took long: " << duration2.toMilliSecondsDouble() << " ms";
        }
 
 
        auto startTime3 = IceUtil::Time::now();
 
        synchronizeObjects();
 
        IceUtil::Time duration3 = IceUtil::Time::now() - startTime3;
        if (duration3.toMilliSecondsDouble() > 10)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << " ObjectSync took long: " << duration3.toMilliSecondsDouble() << " ms";
        }
        ARMARX_TRACE;
 
        this->lastSceneData = this->currentSceneData;
 
        IceUtil::Time duration = IceUtil::Time::now() - startTime;
        synchronize2TimeMS = (float)duration.toMilliSecondsDouble();
 
        return true;
    }
 
    float
    ArmarXPhysicsWorldVisualization::getSyncVisuTime()
    {
        return synchronize2TimeMS;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::loadObject(const ObjectVisuData& obj)
    {
        ARMARX_TRACE;
 
        // try loading from file
        if (not(obj.filename.empty() or obj.project.empty()))
        {
            ARMARX_TRACE;
            return loadObject(obj.filename, obj.project, obj.name);
        }
        ARMARX_TRACE;
 
        // try loading from memory => nope. outdated
 
        ARMARX_TRACE;
 
        // try to ceate primitive
        return createPrimitiveObject(obj.objectPrimitiveData, obj.name);
    }
 
    bool
    ArmarXPhysicsWorldVisualization::loadRobot(const std::string& robotName,
                                               const std::string& robotFile,
                                               const std::string& project,
                                               float scaling,
                                               bool colModel)
    {
        ARMARX_TRACE;
        if (robotFile.empty())
        {
            ARMARX_INFO << deactivateSpam() << "Empty file";
            return false;
        }
 
        if (!project.empty())
        {
            ARMARX_TRACE;
            ARMARX_INFO << "Adding to datapaths of " << project;
            armarx::CMakePackageFinder finder(project);
 
            if (!finder.packageFound())
            {
                ARMARX_ERROR << "ArmarX Package " << project << " has not been found!";
            }
            else
            {
                ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
                armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
        }
        ARMARX_TRACE;
 
        // load robot
        std::string fn = robotFile;
        ArmarXDataPath::getAbsolutePath(fn, fn);
        ARMARX_INFO << "Loading robot from " << fn;
        VirtualRobot::RobotPtr result;
 
        try
        {
            ARMARX_TRACE;
            VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
 
            if (colModel)
            {
                loadMode = VirtualRobot::RobotIO::eCollisionModel;
            }
            result = RobotIO::loadRobot(fn, loadMode);
            ARMARX_DEBUG << "Loading robot done ";
        }
        catch (...)
        {
            ARMARX_IMPORTANT << "Robot loading failed, file:" << fn;
            return false;
        }
        ARMARX_TRACE;
 
        if (scaling != 1.0f)
        {
            ARMARX_INFO << "Scaling robot, factor: " << scaling;
            result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
        }
 
 
        // ensure consistent names
        ARMARX_DEBUG << "Setting up newly loaded robot";
        result->setName(robotName);
        result->setPropagatingJointValuesEnabled(true);
        ARMARX_DEBUG << "adding newly loaded robot to visualization";
        VirtualRobot::SceneObject::VisualizationType visuMode = modelType;
        if (colModel)
        {
            ARMARX_TRACE;
            visuMode = VirtualRobot::SceneObject::Collision;
        }
        addVisualization(result, visuMode);
        return true;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::loadObject(const std::string& objectFile,
                                                const std::string& project,
                                                const std::string& instanceName)
    {
        ARMARX_TRACE;
        if (objectFile.empty())
        {
            ARMARX_INFO << deactivateSpam() << "Empty file";
            return false;
        }
 
        if (!project.empty())
        {
            ARMARX_TRACE;
            ARMARX_INFO << "Adding to datapaths of " << project;
            armarx::CMakePackageFinder finder(project);
 
            if (!finder.packageFound())
            {
                ARMARX_ERROR << "ArmarX Package " << project << " has not been found!";
            }
            else
            {
                ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
                armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
        }
        ARMARX_TRACE;
 
        // load object
        std::string fn = objectFile;
        ArmarXDataPath::getAbsolutePath(fn, fn);
        ARMARX_INFO << "Loading object from " << fn;
 
        ObstaclePtr o;
        if (!o)
        {
            try
            {
                ARMARX_TRACE;
                o = ObjectIO::loadManipulationObject(fn);
            }
            catch (VirtualRobotException& e)
            {
                ARMARX_ERROR << " ERROR while creating manip (file:" << fn << ")\n" << e.what();
                return false;
            }
        }
        if (!o)
        {
            try
            {
                ARMARX_TRACE;
                o = ObjectIO::loadObstacle(fn);
            }
            catch (VirtualRobotException& e)
            {
                //ARMARX_ERROR_S << " ERROR while creating obstacle (file:" << fn << ")" << endl << e.what() ;
                //return false;
            }
        }
        if (!o)
        {
            ARMARX_ERROR << " ERROR while creating (file:" << fn << ")";
            return false;
        }
 
        o->setName(instanceName);
        addVisualization(o, modelType);
        return true;
    }
 
    SceneObjectPtr
    ArmarXPhysicsWorldVisualization::getObject(const std::string& name)
    {
        ARMARX_TRACE;
        for (auto& [object, visu] : addedVisualizations)
        {
            if (object->getName() == name)
            {
                return object;
            }
        }
        return nullptr;
    }
 
    std::vector<RobotPtr>
    ArmarXPhysicsWorldVisualization::getRobots() const
    {
        ARMARX_TRACE;
        std::vector<RobotPtr> result;
        result.reserve(addedRobotVisualizations.size());
        for (auto& r : addedRobotVisualizations)
        {
            result.push_back(r.first);
        }
        return result;
    }
 
    std::vector<SceneObjectPtr>
    ArmarXPhysicsWorldVisualization::getObjects() const
    {
        ARMARX_TRACE;
        std::vector<SceneObjectPtr> result;
        result.reserve(addedVisualizations.size());
        for (auto& r : addedVisualizations)
        {
            result.push_back(r.first);
        }
        return result;
    }
 
    Ice::Long
    ArmarXPhysicsWorldVisualization::getSyncTimestamp() const
    {
        // currentSceneData.timestamp seems to use milliseconds
        return currentSceneData.timestamp * 1000;
    }
 
    bool
    ArmarXPhysicsWorldVisualization::createPrimitiveObject(
        ObjectVisuPrimitivePtr objectPrimitiveData,
        const std::string& name)
    {
        ARMARX_TRACE;
        if (!objectPrimitiveData)
        {
            ARMARX_INFO << deactivateSpam() << "Empty primitive data";
            return false;
        }
 
        VirtualRobot::ObstaclePtr o;
 
        // create object
        switch (objectPrimitiveData->type)
        {
            case Box:
            {
                ARMARX_TRACE;
                BoxVisuPrimitivePtr box = BoxVisuPrimitivePtr::dynamicCast(objectPrimitiveData);
 
                if (!box)
                {
                    ARMARX_ERROR << "Could not cast object...";
                    return false;
                }
 
                VirtualRobot::VisualizationFactory::Color c;
                c.r = box->color.r;
                c.g = box->color.g;
                c.b = box->color.b;
                c.transparency = box->color.a;
                o = VirtualRobot::Obstacle::createBox(box->width, box->height, box->depth, c);
                o->setMass(box->massKG);
            }
            break;
 
            case Sphere:
            {
                ARMARX_TRACE;
                SphereVisuPrimitivePtr s = SphereVisuPrimitivePtr::dynamicCast(objectPrimitiveData);
 
                if (!s)
                {
                    ARMARX_ERROR << "Could not cast object...";
                    return false;
                }
 
                VirtualRobot::VisualizationFactory::Color c;
                c.r = s->color.r;
                c.g = s->color.g;
                c.b = s->color.b;
                c.transparency = s->color.a;
                o = VirtualRobot::Obstacle::createSphere(s->radius, c);
                o->setMass(s->massKG);
            }
            break;
 
            case Cylinder:
            {
                ARMARX_TRACE;
                CylinderVisuPrimitivePtr s =
                    CylinderVisuPrimitivePtr::dynamicCast(objectPrimitiveData);
 
                if (!s)
                {
                    ARMARX_ERROR << "Could not cast object...";
                    return false;
                }
 
                VirtualRobot::VisualizationFactory::Color c;
                c.r = s->color.r;
                c.g = s->color.g;
                c.b = s->color.b;
                c.transparency = s->color.a;
                o = VirtualRobot::Obstacle::createCylinder(s->radius, s->length, c);
                o->setMass(s->massKG);
            }
            break;
 
            default:
                ARMARX_ERROR << "Object type nyi...";
                return false;
        }
        ARMARX_TRACE;
 
        if (!o)
        {
            ARMARX_ERROR << " ERROR while creating object";
            return false;
        }
 
        o->setName(name);
        addVisualization(o, modelType);
        return true;
    }
 
    RobotPtr
    ArmarXPhysicsWorldVisualization::getRobot(const std::string& name)
    {
        ARMARX_TRACE;
        for (auto& r : addedRobotVisualizations)
        {
            if (r.first->getName() == name)
            {
                return r.first;
            }
        }
 
        return RobotPtr();
    }
 
 
} // namespace armarx
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::ArmarXPhysicsWorldVisualization,
                                     ::armarx::ArmarXPhysicsWorldVisualization::GetDefaultName());