AgentInstance.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 "AgentInstance.h"
 
namespace memoryx
{
    AgentInstance::AgentInstance() : Entity()
    {
        setName("");
        setId("");
 
        setPosition(new armarx::FramedPosition());
        setOrientation(new armarx::FramedOrientation());
    }
 
    AgentInstance::AgentInstance(const AgentInstance& source) :
        IceUtil::Shared(source),
        ::armarx::Serializable(source),
        EntityBase(), // dont copy
        AgentInstanceBase(source),
        Entity(source)
    {
    }
 
    AgentInstance::AgentInstance(const std::string& name, const std::string& id) : Entity()
    {
        setName(name);
        setId(id);
 
        setPosition(new armarx::FramedPosition());
        setOrientation(new armarx::FramedOrientation());
    }
 
    EntityAttributeBasePtr
    AgentInstance::getPositionAttribute() const
    {
        return getAttribute("position");
    }
 
    armarx::FramedPositionPtr
    AgentInstance::getPosition() const
    {
        return armarx::FramedPositionPtr::dynamicCast(getPositionBase());
    }
 
    void
    AgentInstance::setPosition(const armarx::FramedPositionBasePtr& position, const ::Ice::Current&)
    {
        putAttribute("position", position);
    }
 
    EntityAttributeBasePtr
    AgentInstance::getOrientationAttribute() const
    {
        return getAttribute("orientation");
    }
 
    armarx::FramedOrientationPtr
    AgentInstance::getOrientation() const
    {
        return armarx::FramedOrientationPtr::dynamicCast(getOrientationBase());
    }
 
    void
    AgentInstance::setOrientation(const armarx::FramedOrientationBasePtr& orientation,
                                  const ::Ice::Current&)
    {
        putAttribute("orientation", orientation);
    }
 
    armarx::FramedPosePtr
    AgentInstance::getPose() const
    {
        auto pos = getPosition();
        return new armarx::FramedPose(pos, getOrientation(), pos->frame, pos->agent);
    }
 
    void
    AgentInstance::setPose(const armarx::FramedPoseBasePtr& newPose)
    {
        armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(newPose);
        setPosition(pose->getPosition());
        setOrientation(pose->getOrientation());
    }
 
    std::string
    AgentInstance::getStringifiedSharedRobotInterfaceProxy(const ::Ice::Current&) const
    {
        return getAttribute("stringifiedSharedRobotInterfaceProxy")->getValue()->getString();
    }
 
    void
    AgentInstance::setStringifiedSharedRobotInterfaceProxy(
        const std::string& stringifiedSharedRobotInterfaceProxy,
        const ::Ice::Current&)
    {
        putAttribute("stringifiedSharedRobotInterfaceProxy", stringifiedSharedRobotInterfaceProxy);
    }
 
    std::string
    AgentInstance::getAgentFilePath(const ::Ice::Current&) const
    {
        return getAttribute("agentFilePath")->getValue()->getString();
    }
 
    void
    AgentInstance::setAgentFilePath(const std::string& agentFilePath, const ::Ice::Current&)
    {
        putAttribute("agentFilePath", agentFilePath);
    }
 
    armarx::SharedRobotInterfacePrx
    AgentInstance::getSharedRobot(const Ice::Current& c) const
    {
        return robot;
    }
 
    void
    AgentInstance::setSharedRobot(const armarx::SharedRobotInterfacePrx& robot,
                                  const Ice::Current& c)
    {
        //        ARMARX_CHECK_EXPRESSION(robot->getName() == getName());
        this->robot = robot;
        setName(robot->getName());
        setStringifiedSharedRobotInterfaceProxy(robot->ice_toString());
    }
 
    Ice::ObjectPtr
    AgentInstance::ice_clone() const
    {
        return this->clone();
    }
 
    AgentInstancePtr
    AgentInstance::clone(const Ice::Current& c) const
    {
        std::shared_lock lock1(entityMutex);
        std::scoped_lock lock2(attributesMutex);
        std::scoped_lock lock3(wrappersMutex);
        AgentInstancePtr ret = new AgentInstance(*this);
        //        ret->deepCopy(*this);
        ret->robot = robot;
        return ret;
    }
 
    // ****************************************************************************
    // Protected members (Ice interface methods overwritten by C++ implementation).
    // ****************************************************************************
 
    armarx::FramedPositionBasePtr
    AgentInstance::getPositionBase(const ::Ice::Current&) const
    {
        return armarx::VariantPtr::dynamicCast(getPositionAttribute()->getValue())
            ->getClass<armarx::FramedPositionBase>();
    }
 
    armarx::FramedOrientationBasePtr
    AgentInstance::getOrientationBase(const ::Ice::Current&) const
    {
        return armarx::VariantPtr::dynamicCast(getOrientationAttribute()->getValue())
            ->getClass<armarx::FramedOrientationBase>();
    }
 
    armarx::FramedPoseBasePtr
    AgentInstance::getPoseBase(const ::Ice::Current&) const
    {
        armarx::FramedOrientationBasePtr ori =
            armarx::VariantPtr::dynamicCast(getOrientationAttribute()->getValue())
                ->getClass<armarx::FramedOrientationBase>();
        armarx::FramedPositionBasePtr pos =
            armarx::VariantPtr::dynamicCast(getPositionAttribute()->getValue())
                ->getClass<armarx::FramedPositionBase>();
        armarx::FramedPoseBasePtr result(
            new armarx::FramedPose(armarx::FramedOrientationPtr::dynamicCast(ori)->toEigen(),
                                   armarx::FramedPositionPtr::dynamicCast(pos)->toEigen(),
                                   pos->frame,
                                   pos->agent));
        return result;
    }
} // namespace memoryx