ObjectInstanceToRobotNodeAttachments.h

Go to the documentation of this file.

#pragma once
 
#include <vector>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/interface/core/RobotState.h>
 
#include <MemoryX/interface/components/ObjectToRobotNodeAttachment.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/ObjectClassSegmentWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
namespace memoryx
{
 
    /**
     * @brief This class can be used to visualize object instances
     * from working memory to ArViz.
     */
    class ObjectInstanceToRobotNodeAttachments
    {
    public:
        ObjectInstanceToRobotNodeAttachments();
 
        /// Set the proxies.
        void initFromProxies(const WorkingMemoryInterfacePrx& workingMemory,
                             const armarx::RobotStateComponentInterfacePrx& robotStateComponent);
 
 
        void attachObjectToRobotNode(const memoryx::AttachObjectToRobotNodeInput& attachment);
        void detachObjectFromRobotNode(const memoryx::DetachObjectFromRobotNodeInput& detachment);
 
        bool isObjectAttached(const std::string& id) const;
 
        Eigen::Matrix4f getObjectPoseInFrame(ObjectInstancePtr object, const std::string& frame);
 
 
        /// Get all entities from `objectInstanceSegment` and cast them to `ObjectInstance`.
        std::vector<ObjectInstancePtr> queryObjects();
 
        ObjectInstancePtr getMatchingObjectInstance(const memoryx::ObjectIdOrClass& object);
        std::string getMatchingObjectID(const memoryx::ObjectIdOrClass& object);
 
 
    private:
        struct Attachment;
 
        Eigen::Matrix4f getAttachedObjectPoseInFrame(const Attachment& attachment,
                                                     const std::string& frame);
        Eigen::Matrix4f getDetachedObjectPoseInFrame(ObjectInstancePtr object,
                                                     const std::string& frame);
 
 
    private:
        armarx::RobotStateComponentInterfacePrx robotStateComponent;
        VirtualRobot::RobotPtr robot;
 
        ObjectInstanceMemorySegmentBasePrx objectInstanceSegment;
 
        struct Attachment
        {
            std::string robotNode;
            Eigen::Matrix4f initialPose = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f objectOffset = Eigen::Matrix4f::Identity();
        };
 
        /// Object instance ID to attachment.
        std::map<std::string, Attachment> attachments;
        std::map<std::string, std::pair<IceUtil::Time, armarx::FramedPose>> objectPoseCache;
    };
 
} // namespace memoryx