ObjectInstancesToArViz.cpp

Go to the documentation of this file.

#include "ObjectInstancesToArViz.h"
 
#include <VirtualRobot/RuntimeEnvironment.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h>
 
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
 
 
namespace memoryx
{
 
    ObjectInstancesToArViz::ObjectInstancesToArViz()
    {
    }
 
 
    void ObjectInstancesToArViz::setArViz(armarx::viz::Client arviz)
    {
        this->arviz = arviz;
    }
 
 
    void ObjectInstancesToArViz::initFromProxies(
        const PriorKnowledgeInterfacePrx& priorKnowledge,
        const WorkingMemoryInterfacePrx& workingMemory,
        const armarx::RobotStateComponentInterfacePrx& robotStateComponent,
        const std::vector<std::string>& datasets)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(priorKnowledge);
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(robotStateComponent);
 
        objectClassSegment.initFromProxy(priorKnowledge, datasets);
        attachments.initFromProxies(workingMemory, robotStateComponent);
 
        this->robotStateComponent = robotStateComponent;
        this->robot = armarx::RemoteRobot::createLocalClone(robotStateComponent, "", {},
                      VirtualRobot::RobotIO::RobotDescription::eStructure);
        this->objectInstanceSegment = workingMemory->getObjectInstancesSegment();
    }
 
 
    void ObjectInstancesToArViz::updateFloorPolygon(const Eigen::Vector2f& extents, float height)
    {
        ARMARX_TRACE;
        arviz.commitLayerContaining(layerNameFloor, makeFloorPolygon(extents, height));
    }
 
    void ObjectInstancesToArViz::updateFloorObject(float height)
    {
        ARMARX_TRACE;
        arviz.commitLayerContaining(layerNameFloor, makeFloorObject().position(Eigen::Vector3f(0, 0, height)));
    }
 
 
    void ObjectInstancesToArViz::updateObjects()
    {
        ARMARX_TRACE;
        /* We are not using the `memoryx::ObjectInstanceWrapper`, since it internally clones a
         * `VirtualRobot::ManipulationObjectPtr`, which is a very costly operation
         * (e.g. 300 ms for 10 objects). */
        std::vector<ObjectInstancePtr> objectInstances = attachments.queryObjects();
 
        updateObjects(objectInstances);
    }
 
    void ObjectInstancesToArViz::updateObjects(const std::vector<ObjectInstancePtr>& objectInstances)
    {
        ARMARX_TRACE;
        armarx::viz::Layer layer = arviz.layer(layerNameObjects);
        for (const auto& object : objectInstances)
        {
            if (auto vizObject = makeObject(object))
            {
                layer.add(*vizObject);
            }
        }
        arviz.commit({layer});
    }
 
 
    armarx::viz::Polygon ObjectInstancesToArViz::makeFloorPolygon(const Eigen::Vector2f& extents, float height)
    {
        ARMARX_TRACE;
        return armarx::viz::Polygon("Floor")
               .plane(Eigen::Vector3f(0, 0, height), Eigen::Quaternionf::Identity(), extents)
               .color(simox::Color::gray(200));
    }
 
 
    armarx::viz::Object ObjectInstancesToArViz::makeFloorObject(const std::string& name)
    {
        ARMARX_TRACE;
        return armarx::viz::Object(name).file("", getFloorObjectFile());
    }
 
 
    std::string ObjectInstancesToArViz::getFloorObjectFile()
    {
        ARMARX_TRACE;
        std::string filename = this->floorObjectFilename;
        VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
        return filename;
    }
 
 
    std::optional<armarx::viz::Object> ObjectInstancesToArViz::makeObject(const ObjectInstancePtr& object)
    {
        ARMARX_TRACE;
        std::string objectName = object->getName();
        const std::string className = object->getMostProbableClass();
        const std::string vizName = objectName + "_" + object->getId();
 
        if (objectClassBlackWhitelist.isExcluded(className))
        {
            return std::nullopt;
        }
 
        // Get filename.
        std::string filename = getClassFilename(className);
        if (filename.empty())
        {
            ARMARX_WARNING << deactivateSpam() << "Could not find object class '" << className << "'. "
                           << "Skipping object '" << object->getName() << "'.";
            return std::nullopt;
        }
 
        // Get global pose.
        Eigen::Matrix4f globalPose = attachments.getObjectPoseInFrame(object, armarx::GlobalFrame);
 
        return armarx::viz::Object(vizName)
               .pose(globalPose)
               .file("", filename);
    }
 
 
    std::string ObjectInstancesToArViz::getClassFilename(const std::string& className)
    {
        ARMARX_TRACE;
        if (auto it = classFilenameCache.find(className); it != classFilenameCache.end())
        {
            // From cache.
            return it->second;
        }
        else
        {
            // Get filename via class object.
            std::optional<ObjectClassWrapper> objectClass = objectClassSegment.getClass(className);
            if (!objectClass)
            {
                return "";
            }
            EntityWrappers::SimoxObjectWrapperPtr wrapper =
                objectClass->classInMemory->getWrapper<EntityWrappers::SimoxObjectWrapper>();
 
            std::string filename = wrapper->getManipulationObjectFileName();
 
            // Update cache.
            classFilenameCache[className] = filename;
 
            return filename;
        }
    }
 
 
    void ObjectInstancesToArViz::updateObjectClassBlackWhitelist(const armarx::BlackWhitelistUpdate& update)
    {
        ARMARX_TRACE;
        armarx::updateBlackWhitelist(objectClassBlackWhitelist, update);
        ARMARX_VERBOSE << "Updated object class black-whitelist: \n" << objectClassBlackWhitelist;
    }
 
 
    void ObjectInstancesToArViz::attachObjectToRobotNode(const AttachObjectToRobotNodeInput& attachment)
    {
        ARMARX_TRACE;
        attachments.attachObjectToRobotNode(attachment);
    }
 
    void ObjectInstancesToArViz::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& detachment)
    {
        ARMARX_TRACE;
        attachments.detachObjectFromRobotNode(detachment);
    }
 
}