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);
            }
        }
    }
 
} // namespace memoryx