LinkedPose.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "LinkedPose.h"
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <Eigen/Geometry>
#include <Eigen/Core>
 
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <Ice/ObjectAdapter.h>
 
 
namespace armarx
{
 
    LinkedPose::LinkedPose() :
        Pose(),
        FramedPose()
    {
        this->referenceRobot = nullptr;
    }
 
    LinkedPose::LinkedPose(const LinkedPose& other) :
        IceUtil::Shared(other),
        armarx::Serializable(other),
        armarx::VariantDataClass(other),
        PoseBase(other),
        FramedPoseBase(other),
        LinkedPoseBase(other),
        Pose(other),
        FramedPose(other)
    {
        if (referenceRobot)
        {
            //ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
            referenceRobot->ref();
        }
    }
 
    LinkedPose::LinkedPose(const FramedPose& other, const SharedRobotInterfacePrx& referenceRobot) :
        IceUtil::Shared(other),
        PoseBase(other),
        FramedPoseBase(other),
        Pose(other),
        FramedPose(other)
    {
        ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
        this->referenceRobot = referenceRobot;
 
        if (referenceRobot)
        {
            //ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
            referenceRobot->ref();
        }
 
    }
 
    LinkedPose::LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
        Pose(m, v),
        FramedPose(m, v, s, referenceRobot->getName())
    {
        ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
        referenceRobot->ref();
        this->referenceRobot = referenceRobot;
    }
 
    LinkedPose::LinkedPose(const Eigen::Matrix4f& m, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
        Pose(m),
        FramedPose(m, s, referenceRobot->getName())
    {
        ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
        referenceRobot->ref();
        this->referenceRobot = referenceRobot;
    }
 
    LinkedPose::~LinkedPose()
    {
        try
        {
            if (referenceRobot)
            {
                referenceRobot->unref();
            }
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
 
    VirtualRobot::LinkedCoordinate LinkedPose::createLinkedCoordinate()
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        VirtualRobot::LinkedCoordinate c(sharedRobot);
        std::string frame = this->getFrame();
 
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
 
        pose.block<3, 3>(0, 0) = QuaternionPtr::dynamicCast(orientation)->toEigen();
        pose.block<3, 1>(0, 3) = Vector3Ptr::dynamicCast(position)->toEigen();
 
        c.set(frame, pose);
 
        return c;
    }
 
    Ice::ObjectPtr LinkedPose::ice_clone() const
    {
        return this->clone();
    }
 
    VariantDataClassPtr LinkedPose::clone(const Ice::Current& c) const
    {
        return new LinkedPose(*this);
    }
 
    std::string LinkedPose::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << FramedPose::output() << std::endl << "reference robot: " << referenceRobot->ice_toString();
        return s.str();
    }
 
    VariantTypeId LinkedPose::getType(const Ice::Current& c) const
    {
        return VariantType::LinkedPose;
    }
 
    bool LinkedPose::validate(const Ice::Current& c)
    {
        return true;
    }
 
    void LinkedPose::changeFrame(const std::string& newFrame, const Ice::Current& c)
    {
        FramedPose::changeFrame(referenceRobot, newFrame);
    }
 
    void LinkedPose::changeToGlobal()
    {
        FramedPose::changeToGlobal(referenceRobot);
    }
 
    LinkedPosePtr LinkedPose::toGlobal() const
    {
        FramedPosePtr fp = this->FramedPose::toGlobal(referenceRobot);
        LinkedPosePtr newPose = new LinkedPose(fp->toEigen(), fp->frame, referenceRobot);
        return newPose;
    }
 
 
    void LinkedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Pose::serialize(obj, c);
        obj->setString("referenceRobot", "");
    }
 
    void LinkedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        FramedPose::deserialize(obj);
 
        std::string remoteRobotId = obj->getString("referenceRobot");
        referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
 
        if (!referenceRobot)
        {
            ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
        }
    }
 
    void LinkedPose::ice_postUnmarshal()
    {
        if (referenceRobot)
        {
            //ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
            referenceRobot->ref();
        }
 
        FramedPose::ice_postUnmarshal();
    }
 
 
    LinkedDirection::LinkedDirection()
        = default;
 
    LinkedDirection::LinkedDirection(const LinkedDirection& source) :
        IceUtil::Shared(source),
        armarx::Serializable(source),
        Vector3Base(source),
        FramedDirectionBase(source),
        LinkedDirectionBase(source),
        Vector3(source),
        FramedDirection(source)
    {
        referenceRobot = source.referenceRobot;
 
        if (referenceRobot)
        {
            //ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
            referenceRobot->ref();
        }
    }
 
    LinkedDirection::LinkedDirection(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot) :
        FramedDirection(v, frame, referenceRobot->getName())
    {
        referenceRobot->ref();
        this->referenceRobot = referenceRobot;
    }
 
    LinkedDirection::~LinkedDirection()
    {
        try
        {
            if (referenceRobot)
            {
                referenceRobot->unref();
            }
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void LinkedDirection::changeFrame(const std::string& newFrame, const Ice::Current& c)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
 
        FramedDirectionPtr frVec = ChangeFrame(sharedRobot, *this, newFrame);
        x = frVec->x;
        y = frVec->y;
        z = frVec->z;
        frame = frVec->frame;
    }
 
 
    void LinkedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
    }
 
    void LinkedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        throw LocalException("LinkedDirection cannot be deserialized! Deserialize FramedDirection");
    }
 
    void LinkedDirection::ice_postUnmarshal()
    {
        if (referenceRobot)
        {
            //            ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
            referenceRobot->ref();
        }
 
        FramedDirection::ice_postUnmarshal();
    }
 
 
    void VariantType::suppressWarningUnusedVariableForLinkedPoseAndDirection()
    {
        ARMARX_DEBUG_S << VAROUT(LinkedPose);
        ARMARX_DEBUG_S << VAROUT(LinkedDirection);
    }
 
}