SharedRobotServants.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 "SharedRobotServants.h"
 
#include <Eigen/Geometry>
 
#include <Ice/ObjectAdapter.h>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
using namespace VirtualRobot;
using namespace Eigen;
using namespace Ice;
 
 
#undef VERBOSE
 
//#define VERBOSE
 
 
namespace armarx
{
 
    using NodeCache = std::map<std::string, SharedRobotNodeInterfacePrx>;
 
    ///////////////////////////////
    // SharedObjectBase
    ///////////////////////////////
 
    SharedObjectBase::SharedObjectBase()
    {
        this->_referenceCount = 0;
#ifdef VERBOSE
        ARMARX_LOG_S << "construct " << this << flush;
#endif
    }
 
    void
    SharedObjectBase::ref(const Current& current)
    {
        std::unique_lock lock(this->_counterMutex);
 
        _referenceCount++;
 
#ifdef VERBOSE
        ARMARX_LOG_S << "ref: " << _referenceCount << " " << this << flush;
#endif
    }
 
    void
    SharedObjectBase::unref(const Current& current)
    {
        std::unique_lock lock(this->_counterMutex);
 
#ifdef VERBOSE
        ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
#endif
        _referenceCount--;
 
        if (_referenceCount == 0)
        {
            this->destroy(current);
        }
    }
 
    void
    SharedObjectBase::destroy(const Current& current)
    {
        try
        {
            current.adapter->remove(current.id);
#ifdef VERBOSE
            ARMARX_ERROR_S << "[SharedObject] destroy "
                           << " " << this << flush;
#endif
        }
        catch (const NotRegisteredException& e)
        {
            //        ARMARX_INFO_S << "destroy failed with: " << e.what();
            throw ObjectNotExistException(__FILE__, __LINE__);
        };
    }
 
    ///////////////////////////////
    // SharedRobotNodeServant
    ///////////////////////////////
 
    SharedRobotNodeServant::SharedRobotNodeServant(RobotNodePtr node) : _node(node)
    {
#ifdef VERBOSE
        ARMARX_LOG_S << "[SharedRobotNodeServant] construct "
                     << " " << this << flush;
#endif
    }
 
    SharedRobotNodeServant::~SharedRobotNodeServant()
    {
#ifdef VERBOSE
        ARMARX_FATAL_S << "[SharedRobotNodeServant] destruct "
                       << " " << this << flush;
#endif
    }
 
    float
    SharedRobotNodeServant::getJointValue(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return _node->getJointValue();
    }
 
    std::string
    SharedRobotNodeServant::getName(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return _node->getName();
    }
 
    PoseBasePtr
    SharedRobotNodeServant::getLocalTransformation(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return new Pose(_node->getLocalTransformation());
    }
 
    FramedPoseBasePtr
    SharedRobotNodeServant::getGlobalPose(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return new FramedPose(_node->getGlobalPose(), GlobalFrame, "");
    }
 
    FramedPoseBasePtr
    SharedRobotNodeServant::getPoseInRootFrame(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return new FramedPose(_node->getPoseInRootFrame(),
                              _node->getRobot()->getRootNode()->getName(),
                              _node->getRobot()->getName());
    }
 
    JointType
    SharedRobotNodeServant::getType(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
 
        if (_node->isRotationalJoint())
        {
            return eRevolute;
        }
        else if (_node->isTranslationalJoint())
        {
            return ePrismatic;
        }
        else
        {
            return eFixed;
        }
    }
 
    Vector3BasePtr
    SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        RobotNodePrismatic* prismatic = dynamic_cast<RobotNodePrismatic*>(_node.get());
 
        if (prismatic)
        {
            return new Vector3(prismatic->getJointTranslationDirection());
        }
        else
        {
            return new Vector3;
        }
    }
 
    Vector3BasePtr
    SharedRobotNodeServant::getJointRotationAxis(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        RobotNodeRevolute* revolute = dynamic_cast<RobotNodeRevolute*>(_node.get());
 
        if (revolute)
        {
            return new Vector3(revolute->getJointRotationAxis());
        }
        else
        {
            return new Vector3;
        }
    }
 
    bool
    SharedRobotNodeServant::hasChild(const std::string& name,
                                     bool recursive,
                                     const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        //return _node->hasChild(name,recursive);
        return false;
    }
 
    std::string
    SharedRobotNodeServant::getParent(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        SceneObjectPtr parent = _node->getParent();
 
        if (!parent)
        {
            throw UserException("This RobotNode has no parent.");
        }
 
        return parent->getName();
    }
 
    NameList
    SharedRobotNodeServant::getChildren(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        std::vector<SceneObjectPtr> children = _node->getChildren();
        NameList names;
        for (SceneObjectPtr const& node : children)
        {
            names.push_back(node->getName());
        }
        return names;
    }
 
    NameList
    SharedRobotNodeServant::getAllParents(const std::string& name,
                                          const Ice::Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        std::vector<RobotNodePtr> parents =
            _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
        NameList names;
        for (RobotNodePtr const& node : parents)
        {
            names.push_back(node->getName());
        }
        return names;
    }
 
    float
    SharedRobotNodeServant::getJointValueOffest(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return _node->getJointValueOffset();
    }
 
    float
    SharedRobotNodeServant::getJointLimitHigh(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return _node->getJointLimitHigh();
    }
 
    float
    SharedRobotNodeServant::getJointLimitLow(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return _node->getJointLimitLow();
    }
 
    Vector3BasePtr
    SharedRobotNodeServant::getCoM(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return new Vector3(_node->getCoMLocal());
    }
 
    FloatSeq
    SharedRobotNodeServant::getInertia(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        FloatSeq result;
 
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
            {
                result.push_back(_node->getInertiaMatrix()(i, j));
            }
 
        return result;
    }
 
    float
    SharedRobotNodeServant::getMass(const Current& current) const
    {
        ReadLockPtr lock = _node->getRobot()->getReadLock();
        return _node->getMass();
    }
 
    ///////////////////////////////
    // SharedRobotServant
    ///////////////////////////////
 
    SharedRobotServant::SharedRobotServant(RobotPtr robot,
                                           RobotStateComponentInterfacePrx robotStateComponent,
                                           RobotStateListenerInterfacePrx robotStateListenerPrx) :
        _robot(robot),
        robotStateComponent(robotStateComponent),
        robotStateListenerPrx(robotStateListenerPrx)
    {
#ifdef VERBOSE
        ARMARX_WARNING_S << "construct " << this << flush;
#endif
    }
 
    SharedRobotServant::~SharedRobotServant()
    {
#ifdef VERBOSE
        ARMARX_WARNING_S << "destruct " << this << flush;
#endif
        std::unique_lock cloneLock(m);
 
        for (auto value : this->_cachedNodes)
        {
            try
            {
                value.second->unref();
            }
            catch (...)
            {
            }
        }
    }
 
    void
    SharedRobotServant::setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent)
    {
        this->robotStateComponent = robotStateComponent;
    }
 
    SharedRobotNodeInterfacePrx
    SharedRobotServant::getRobotNode(const std::string& name, const Current& current)
    {
        //    ARMARX_LOG_S << "Looking for node: " << name << flush;
        assert(_robot);
        std::unique_lock cloneLock(m);
        SharedRobotNodeInterfacePrx prx;
 
        if (this->_cachedNodes.find(name) == this->_cachedNodes.end())
        {
            RobotNodePtr robotNode = _robot->getRobotNode(name);
 
            if (!robotNode)
            {
                ARMARX_WARNING_S << "RobotNode \"" + name + "\" not defined.";
                throw UserException("RobotNode \"" + name + "\" not defined.");
            }
 
            SharedRobotNodeInterfacePtr servant =
                new SharedRobotNodeServant(_robot->getRobotNode(name));
            //servant->ref();
            prx = SharedRobotNodeInterfacePrx::uncheckedCast(current.adapter->addWithUUID(servant));
            prx->ref();
            //        return prx;
            this->_cachedNodes[name] = prx;
        }
 
        return this->_cachedNodes[name];
    }
 
    SharedRobotNodeInterfacePrx
    SharedRobotServant::getRootNode(const Current& current)
    {
        assert(_robot);
        std::unique_lock cloneLock(m);
        std::string name = _robot->getRootNode() /*,current*/->getName();
        return this->getRobotNode(name, current);
    }
 
    bool
    SharedRobotServant::hasRobotNode(const std::string& name, const Current& current)
    {
        return _robot->hasRobotNode(name);
    }
 
    NameList
    SharedRobotServant::getRobotNodes(const Current& current)
    {
        std::vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
        NameList names;
        for (RobotNodePtr const& node : robotNodes)
        {
            names.push_back(node->getName());
        }
        return names;
    }
 
    RobotNodeSetInfoPtr
    SharedRobotServant::getRobotNodeSet(const std::string& name, const Current& current)
    {
        RobotNodeSetPtr robotNodeSet = _robot->getRobotNodeSet(name);
 
        if (!robotNodeSet)
        {
            throw UserException("RobotNodeSet \"" + name + "\" not defined");
        }
 
        std::vector<RobotNodePtr> robotNodes = robotNodeSet->getAllRobotNodes();
        NameList names;
        for (RobotNodePtr const& node : robotNodes)
        {
            names.push_back(node->getName());
        }
 
        RobotNodeSetInfoPtr info = new RobotNodeSetInfo;
        info->names = names;
        info->name = name;
        info->tcpName = robotNodeSet->getTCP()->getName();
        info->rootName = robotNodeSet->getKinematicRoot()->getName();
 
        return info;
    }
 
    NameList
    SharedRobotServant::getRobotNodeSets(const Current& current)
    {
        std::vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
        NameList names;
        for (RobotNodeSetPtr const& set : robotNodeSets)
        {
            names.push_back(set->getName());
        }
 
        return names;
    }
 
    bool
    SharedRobotServant::hasRobotNodeSet(const std::string& name, const Current& current)
    {
        return _robot->hasRobotNodeSet(name);
    }
 
    std::string
    SharedRobotServant::getName(const Current& current)
    {
        //ARMARX_VERBOSE_S << "SharedRobotServant::getname:" << _robot->getName();
        return _robot->getName();
    }
 
    std::string
    SharedRobotServant::getType(const Current& current)
    {
        return _robot->getType();
    }
 
    void
    SharedRobotServant::setTimestamp(const IceUtil::Time& updateTime)
    {
        updateTimestamp = updateTime;
    }
 
    PoseBasePtr
    SharedRobotServant::getGlobalPose(const Current& current)
    {
        ReadLockPtr lock = _robot->getReadLock();
        return new Pose(_robot->getGlobalPose());
    }
 
    NameValueMap
    SharedRobotServant::getConfig(const Current& current)
    {
        if (!_robot)
        {
            ARMARX_WARNING_S << "no robot set";
            return NameValueMap();
        }
 
        ReadLockPtr lock = _robot->getReadLock();
        std::map<std::string, float> values = _robot->getConfig()->getRobotNodeJointValueMap();
        NameValueMap result(values.begin(), values.end());
        return result;
    }
 
    NameValueMap
    SharedRobotServant::getConfigAndPose(PoseBasePtr& globalPose, const Current& current)
    {
        globalPose = getGlobalPose(current);
        return getConfig(current);
    }
 
    TimestampBasePtr
    SharedRobotServant::getTimestamp(const Current&) const
    {
        return new TimestampVariant(updateTimestamp);
    }
 
    RobotStateComponentInterfacePrx
    SharedRobotServant::getRobotStateComponent(const Current&) const
    {
        return robotStateComponent;
    }
 
    void
    SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr& pose, const Current&)
    {
        WriteLockPtr lock = _robot->getWriteLock();
        Eigen::Matrix4f newPose = PosePtr::dynamicCast(pose)->toEigen();
        if (robotStateListenerPrx)
        {
            Eigen::Matrix4f oldPose = _robot->getGlobalPose();
            robotStateListenerPrx->reportGlobalRobotRootPose(
                new FramedPose(newPose, GlobalFrame, ""),
                TimeUtil::GetTime().toMicroSeconds(),
                !oldPose.isApprox(newPose));
        }
        _robot->setGlobalPose(newPose, true);
    }
 
    float
    SharedRobotServant::getScaling(const Current&)
    {
        return _robot->getScaling();
    }
 
    RobotPtr
    SharedRobotServant::getRobot() const
    {
        return this->_robot;
    }
 
} // namespace armarx