AgentInstancesSegment.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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    MemoryX::WorkingMemory
* @author     Thomas von der Heyde (tvh242 at hotmail dot com)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "AgentInstancesSegment.h"
 
namespace memoryx
{
    AgentInstancesSegment::AgentInstancesSegment(armarx::IceManagerPtr ic) :
        WorkingMemoryEntitySegment<AgentInstance>::WorkingMemoryEntitySegment(),
        AgentInstancesSegmentBase::AgentInstancesSegmentBase(),
        ic(ic)
    {}
 
    AgentInstanceBaseList AgentInstancesSegment::getAllAgentInstances(const ::Ice::Current&) const
    {
        AgentInstanceBaseList result;
        auto allEntities = getAllEntities();
        result.reserve(allEntities.size());
 
        for (auto&& entity : allEntities)
        {
            AgentInstanceBasePtr agentEntity = AgentInstanceBasePtr::dynamicCast(std::move(entity));
 
            if (!agentEntity)
            {
                ARMARX_WARNING_S << "Entity with id " << entity->getId() << " is not of type AgentInstance!";
                continue;
            }
 
            result.push_back(std::move(agentEntity));
        }
 
        return result;
    }
 
    AgentInstanceBasePtr AgentInstancesSegment::getAgentInstanceById(const std::string& id, const::Ice::Current&) const
    {
        auto entity = getEntityById(id);
        if (!entity)
        {
            return NULL;
        }
        AgentInstanceBasePtr res = AgentInstanceBasePtr::dynamicCast(entity);
 
        if (!res)
        {
            ARMARX_WARNING_S << "Entity with id " << id << " is not of type AgentInstance!";
        }
 
        return res;
    }
 
    AgentInstanceBasePtr AgentInstancesSegment::getAgentInstanceByName(const std::string& name, const::Ice::Current&) const
    {
        auto entity = getEntityByName(name);
        if (!entity)
        {
            return NULL;
        }
        AgentInstanceBasePtr res = AgentInstanceBasePtr::dynamicCast(entity);
 
        if (!res)
        {
            ARMARX_WARNING_S << "Entity with name '" << name << "' is not of type AgentEntity!";
        }
 
        return res;
    }
 
    armarx::FramedPoseBasePtr AgentInstancesSegment::convertToWorldPose(const std::string& agentName, const armarx::FramedPoseBasePtr& localPose, const Ice::Current& c) const
    {
        if (localPose->frame == armarx::GlobalFrame)
        {
            return localPose;
        }
 
        auto agent = getAgentInstanceByName(agentName);
 
        if (!agent)
        {
            ARMARX_ERROR_S << "no agent with name " << agentName;
            return NULL;
        }
 
        armarx::FramedPositionPtr agentPosition = armarx::FramedPositionPtr::dynamicCast(agent->getPositionBase());
        armarx::FramedOrientationPtr agentOrientation = armarx::FramedOrientationPtr::dynamicCast(agent->getOrientationBase());
        armarx::PosePtr agentPose = new armarx::Pose(agentOrientation->toEigen(), agentPosition->toEigen());
        armarx::FramedPosePtr localPoseCast = armarx::FramedPosePtr::dynamicCast(localPose);
 
        //        auto basePrx = ic->getCommunicator()->stringToProxy(agent->getStringifiedSharedRobotInterfaceProxy());
        //        SharedRobotInterfacePrx robotPrx = SharedRobotInterfacePrx::checkedCast(basePrx);
        auto robotPrx = agent->getSharedRobot();
 
        if (!robotPrx)
        {
            ARMARX_ERROR_S << "Could not create proxy to robot state  - proxy string was : " << agent->getStringifiedSharedRobotInterfaceProxy() << " for agent " << agentName;
            return NULL;
        }
 
        localPoseCast->changeFrame(robotPrx, robotPrx->getRootNode()->getName());
        Eigen::Matrix4f objectPoseGlobal = agentPose->toEigen() * localPoseCast->toEigen();
 
        armarx::FramedPosePtr p(new armarx::FramedPose(objectPoseGlobal, armarx::GlobalFrame, ""));
        return p;
    }
 
    armarx::FramedPoseBasePtr AgentInstancesSegment::convertToLocalPose(const std::string& agentName, const armarx::PoseBasePtr& worldPose, const std::string& targetFrame, const Ice::Current& c) const
    {
        auto agent = getAgentInstanceByName(agentName);
 
        if (!agent)
        {
            return NULL;
        }
 
        armarx::FramedPositionPtr agentPosition = armarx::FramedPositionPtr::dynamicCast(agent->getPositionBase());
        armarx::FramedOrientationPtr agentOrientation = armarx::FramedOrientationPtr::dynamicCast(agent->getOrientationBase());
        armarx::PosePtr agentPose = new armarx::Pose(agentOrientation->toEigen(), agentPosition->toEigen());
        armarx::FramedPosePtr worldPoseCast = armarx::FramedPosePtr::dynamicCast(worldPose);
 
        //        auto basePrx = ic->getCommunicator()->stringToProxy(agent->getStringifiedSharedRobotInterfaceProxy());
        //        SharedRobotInterfacePrx robotPrx = SharedRobotInterfacePrx::checkedCast(basePrx);
        auto robotPrx = agent->getSharedRobot();
 
        if (!robotPrx)
        {
            ARMARX_ERROR_S << "Could not create proxy to robot state  - proxy string was : " << agent->getStringifiedSharedRobotInterfaceProxy();
            return NULL;
        }
 
        worldPoseCast->changeFrame(robotPrx, robotPrx->getRootNode()->getName());
        Eigen::Matrix4f objectPoseLocal = worldPoseCast->toEigen() * agentPose->toEigen().inverse();
 
        armarx::FramedPosePtr p(new armarx::FramedPose(objectPoseLocal, robotPrx->getRootNode()->getName(), robotPrx->getName()));
        p->changeFrame(robotPrx, targetFrame);
        return p;
    }
 
    std::string AgentInstancesSegment::addEntity(const EntityBasePtr& entity, const Ice::Current&)
    {
 
        setRemoteRobotPose(entity);
 
        std::string result = WorkingMemoryEntitySegment::addEntity(entity);
        return result;
    }
 
    void AgentInstancesSegment::updateEntity(const std::string& id, const EntityBasePtr& entity, const Ice::Current&)
    {
 
        setRemoteRobotPose(entity);
 
        WorkingMemoryEntitySegment::updateEntity(id, entity);
    }
 
    std::string AgentInstancesSegment::upsertEntity(const std::string& entityId, const EntityBasePtr& newEntity, const ::Ice::Current&)
    {
        setRemoteRobotPose(newEntity);
        return WorkingMemoryEntitySegment::upsertEntity(entityId, newEntity);
    }
 
    std::string AgentInstancesSegment::upsertEntityByName(const std::string& entityName, const EntityBasePtr& newEntity, const ::Ice::Current&)
    {
        setRemoteRobotPose(newEntity);
        return WorkingMemoryEntitySegment::upsertEntityByName(entityName, newEntity);
    }
 
    void AgentInstancesSegment::removeEntity(const std::string& id, const Ice::Current&)
    {
        ARMARX_IMPORTANT_S << "Remove agent instance " << id;
 
        WorkingMemoryEntitySegment::removeEntity(id);
 
    }
 
    void AgentInstancesSegment::removeAllEntities(const Ice::Current&)
    {
        ARMARX_DEBUG_S << " removeAllEntities ";
        WorkingMemoryEntitySegment::removeAllEntities();
 
    }
 
    void AgentInstancesSegment::setRemoteRobotPose(EntityBasePtr entity)
    {
        AgentInstancePtr agentInstance = AgentInstancePtr::dynamicCast(entity);
 
        if (agentInstance)
        {
            //            ARMARX_DEBUG << "Update agent instance " << agentInstance->getName();
            auto robotPrx = agentInstance->getSharedRobot();
 
            if (robotPrx)
            {
                ARMARX_CHECK_EXPRESSION(agentInstance->getPosition()->getFrame() == armarx::GlobalFrame);
                ARMARX_CHECK_EXPRESSION(agentInstance->getOrientation()->getFrame() == armarx::GlobalFrame);
                armarx::PosePtr pose = new armarx::Pose(agentInstance->getPosition(), agentInstance->getOrientation());
                robotPrx->setGlobalPose(pose);
            }
        }
    }
 
}