ObjectInstancesToArViz.h

Go to the documentation of this file.

#pragma once
 
#include <set>
#include <vector>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h>
#include <RobotAPI/interface/core/RobotState.h>
 
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/components/WorkingMemoryToArVizInterface.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/ObjectClassSegmentWrapper.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/ObjectInstanceToRobotNodeAttachments.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
// Todo: Move somewhere else
#include <RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h>
 
namespace memoryx
{
 
    /**
     * @brief This class can be used to visualize object instances
     * from working memory to ArViz.
     */
    class ObjectInstancesToArViz
    {
    public:
        ObjectInstancesToArViz();
 
        /// Set the ArViz client.
        void setArViz(armarx::viz::Client arviz);
        /// Set the proxies.
        void initFromProxies(const PriorKnowledgeInterfacePrx& priorKnowledge,
                             const WorkingMemoryInterfacePrx& workingMemory,
                             const armarx::RobotStateComponentInterfacePrx& robotStateComponent,
                             const std::vector<std::string>& datasets);
 
 
        /// Draw a the floor as a polygon.
        /// @see `makeFloorPolygon()`
        void updateFloorPolygon(const Eigen::Vector2f& extents = {10000, 10000}, float height = 0);
        /// Draw a the floor as a simox object.
        /// @see `makeFloorObject()`
        void updateFloorObject(float height = 0);
 
 
        /// Query object instances and update their visualization.
        void updateObjects();
        /// Update the object instances' visualizations.
        void updateObjects(const std::vector<ObjectInstancePtr>& objectInstances);
 
        /// Make a polygon representing the floor.
        armarx::viz::Polygon makeFloorPolygon(const Eigen::Vector2f& extents = {10000, 10000},
                                              float height = 0);
 
        armarx::viz::Object makeFloorObject(const std::string& name = "Floor");
        std::string getFloorObjectFile();
 
        /// Make a `armarx::viz::Object` from a `memoryx::ObjectInstance`.
        std::optional<armarx::viz::Object> makeObject(const ObjectInstancePtr& object);
 
 
        void updateObjectClassBlackWhitelist(const armarx::BlackWhitelistUpdate& update);
 
        void attachObjectToRobotNode(const memoryx::AttachObjectToRobotNodeInput& attachment);
 
        void detachObjectFromRobotNode(const memoryx::DetachObjectFromRobotNodeInput& detachment);
 
 
    public:
        std::string layerNameFloor = "Floor";
        std::string layerNameObjects = "ObjectInstances";
 
        std::string floorObjectFilename = "images/Floor20x20.xml"; // In VirtualRobot/data
 
 
    private:
        std::string getClassFilename(const std::string& className);
 
 
    private:
        armarx::viz::Client arviz;
 
        armarx::RobotStateComponentInterfacePrx robotStateComponent;
        VirtualRobot::RobotPtr robot;
 
        ObjectInstanceMemorySegmentBasePrx objectInstanceSegment;
        ObjectClassSegmentWrapper objectClassSegment;
 
        /// Caches [class name -> filename] entries.
        std::map<std::string, std::string> classFilenameCache;
 
 
        armarx::StringBlackWhitelist objectClassBlackWhitelist;
 
        ObjectInstanceToRobotNodeAttachments attachments;
    };
 
} // namespace memoryx