ObjectInstanceToRobotNodeAttachments.cpp

Go to the documentation of this file.

#include "ObjectInstanceToRobotNodeAttachments.h"
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h>
 
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 
namespace memoryx
{
 
    ObjectInstanceToRobotNodeAttachments::ObjectInstanceToRobotNodeAttachments()
    {
    }
 
 
    void ObjectInstanceToRobotNodeAttachments::initFromProxies(
        const WorkingMemoryInterfacePrx& workingMemory,
        const armarx::RobotStateComponentInterfacePrx& robotStateComponent)
    {
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(robotStateComponent);
 
        this->robotStateComponent = robotStateComponent;
        this->robot = armarx::RemoteRobot::createLocalClone(robotStateComponent, "", {},
                      VirtualRobot::RobotIO::RobotDescription::eStructure);
        this->objectInstanceSegment = workingMemory->getObjectInstancesSegment();
    }
 
 
    void ObjectInstanceToRobotNodeAttachments::attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input)
    {
        ObjectInstancePtr instance = getMatchingObjectInstance(input.object);
        if (!instance)
        {
            ARMARX_WARNING << "No object instance with ID '" << input.object.memoryID << "' or class '"
                           << input.object.className << "' found.";
            return;
        }
 
        ARMARX_VERBOSE << "Attaching object '" << instance->getId() << "' (class '" << instance->getMostProbableClass() << "')"
                       << " to robot node '" << input.robotNodeName << "'.";
 
        Attachment attachment;
        attachment.robotNode = input.robotNodeName;
        if (input.useCurrentPose)
        {
            // Get pose in robot node frame.
            attachment.initialPose = getObjectPoseInFrame(instance, attachment.robotNode);
        }
        if (input.objectOffset)
        {
            attachment.objectOffset = armarx::PosePtr::dynamicCast(input.objectOffset)->toEigen();
        }
 
        this->attachments[getMatchingObjectID(input.object)] = attachment;
    }
 
    void ObjectInstanceToRobotNodeAttachments::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& detachment)
    {
        std::string id = getMatchingObjectID(detachment.object);
        if (id.empty())
        {
            ARMARX_WARNING << "No object instance with ID '" << detachment.object.memoryID << "' or class '"
                           << detachment.object.className << "' found.";
            return;
        }
 
        if (auto it = attachments.find(id); it != attachments.end())
        {
            ARMARX_VERBOSE << "Detaching object '" << id << "' from robot node '" << it->second.robotNode << "'.";
            attachments.erase(it);
        }
        else
        {
            ARMARX_VERBOSE << "No attachment of object '" << id << "' found.";
        }
    }
 
 
 
    std::vector<ObjectInstancePtr> ObjectInstanceToRobotNodeAttachments::queryObjects()
    {
        EntityBaseList entities = objectInstanceSegment->getAllEntities();
 
        std::vector<ObjectInstancePtr> instances;
        for (const auto& entity : entities)
        {
            ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entity);
            if (object)
            {
                instances.push_back(object);
            }
        }
 
        return instances;
    }
 
    Eigen::Matrix4f ObjectInstanceToRobotNodeAttachments::getObjectPoseInFrame(
        ObjectInstancePtr object, const std::string& frame)
    {
        if (auto it = attachments.find(object->getId()); it != attachments.end())
        {
            return getAttachedObjectPoseInFrame(it->second, frame);
        }
        else
        {
            return getDetachedObjectPoseInFrame(object, armarx::GlobalFrame);
        }
    }
 
    Eigen::Matrix4f ObjectInstanceToRobotNodeAttachments::getAttachedObjectPoseInFrame(const Attachment& attachment, const std::string& frame)
    {
        armarx::RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
 
        armarx::FramedPose framed(attachment.initialPose * attachment.objectOffset,
                                  attachment.robotNode, robot->getName());
 
        if (frame == armarx::GlobalFrame)
        {
            return framed.toGlobalEigen(robot);
        }
        else
        {
            framed.changeFrame(robot, frame);
            return framed.toEigen();
        }
    }
 
    Eigen::Matrix4f ObjectInstanceToRobotNodeAttachments::getDetachedObjectPoseInFrame(
        ObjectInstancePtr object, const std::string& frame)
    {
        if (object->getPose()->getFrame() == frame)
        {
            return object->getPose()->toEigen();
        }
 
        // Transform by syncing robot.
        auto it = objectPoseCache.find(object->getId());
        if (it != objectPoseCache.end() && object->hasLocalizationTimestamp())
        {
            const IceUtil::Time localizationTimestamp = object->getLocalizationTimestamp();
            const IceUtil::Time previousTimestamp = it->second.first;
            if (previousTimestamp >= localizationTimestamp)
            {
                // Stored pose is up to date.
                armarx::FramedPose& pose = it->second.second;
                // Transform in-place if necessary.
                if (pose.getFrame() != frame)
                {
                    armarx::RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
                    pose.changeFrame(robot, frame);
                }
                return pose.toEigen();
            }
        }
 
        armarx::RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
        const IceUtil::Time ts = object->hasLocalizationTimestamp()
                                 ? object->getLocalizationTimestamp()
                                 : armarx::TimeUtil::GetTime();
 
        armarx::FramedPose pose = *object->getPose();
        if (pose.getFrame() != frame)
        {
            armarx::RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
            pose.changeFrame(robot, frame);
        }
        // Store it in the cache so we don't recompute the framed pose on the next (spurious) update.
        objectPoseCache[object->getId()] = std::make_pair(ts, pose);
        return pose.toEigen();
    }
 
 
 
    bool ObjectInstanceToRobotNodeAttachments::isObjectAttached(const std::string& id) const
    {
        return attachments.count(id) > 0;
    }
 
    std::string ObjectInstanceToRobotNodeAttachments::getMatchingObjectID(const ObjectIdOrClass& object)
    {
        if (!object.memoryID.empty())
        {
            return object.memoryID;
        }
        else if (auto instance = getMatchingObjectInstance(object))
        {
            return instance->getId();
        }
        else
        {
            return "";
        }
    }
 
    ObjectInstancePtr ObjectInstanceToRobotNodeAttachments::getMatchingObjectInstance(const ObjectIdOrClass& object)
    {
        if (!object.memoryID.empty())
        {
            // Look for instance with ID.
            for (ObjectInstancePtr instance : queryObjects())
            {
                if (instance && instance->getId() == object.memoryID)
                {
                    return instance;
                }
            }
            return nullptr;
        }
        else
        {
            // Look for instance with class name.
            for (ObjectInstancePtr instance : queryObjects())
            {
                if (instance && instance->getMostProbableClass() == object.className)
                {
                    return instance;
                }
            }
            return nullptr;
        }
    }
 
}