TCPControlUnitObserver.cpp

/*
* 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    ArmarX::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/

#include "TCPControlUnitObserver.h"

//#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>

#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"

namespace armarx
{

    TCPControlUnitObserver::TCPControlUnitObserver()
    {
    }

    void TCPControlUnitObserver::onInitObserver()
    {
        usingTopic(getProperty<std::string>("TCPControlUnitName").getValue() + "State");

        // register all checks
        offerConditionCheck("updated", new ConditionCheckUpdated());
        offerConditionCheck("larger", new ConditionCheckLarger());
        offerConditionCheck("equals", new ConditionCheckEquals());
        offerConditionCheck("smaller", new ConditionCheckSmaller());
        //    offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
        offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
        offerConditionCheck("approx", new ConditionCheckApproxPose());



    }

    void TCPControlUnitObserver::onConnectObserver()
    {
        offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
        offerChannel(TCP_TRANS_VELOCITIES_CHANNEL, "TCP velocities of the robot.");

    }

    PropertyDefinitionsPtr TCPControlUnitObserver::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new TCPControlUnitObserverPropertyDefinitions(
                                          getConfigIdentifier()));
    }

    void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
    {
        std::unique_lock lock(dataMutex);
        //        ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
        FramedPoseBaseMap::const_iterator it = poseMap.begin();

        for (; it != poseMap.end(); it++)
        {

            FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
            const std::string& tcpName = it->first;

            if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
            {
                offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
            }
            else
            {
                setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
            }

            updateChannel(TCP_POSE_CHANNEL);

            if (!existsChannel(tcpName))
            {
                offerChannel(tcpName, "pose components of " + tcpName);
                offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
                offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
                offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
                offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
                offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
                offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
                offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
                offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
            }
            else
            {
                setDataField(tcpName, "x", Variant(vec->position->x));
                setDataField(tcpName, "y", Variant(vec->position->y));
                setDataField(tcpName, "z", Variant(vec->position->z));
                setDataField(tcpName, "qx", Variant(vec->orientation->qx));
                setDataField(tcpName, "qy", Variant(vec->orientation->qy));
                setDataField(tcpName, "qz", Variant(vec->orientation->qz));
                setDataField(tcpName, "qw", Variant(vec->orientation->qw));
                setDataField(tcpName, "frame", Variant(vec->frame));
            }

            updateChannel(tcpName);

        }
    }

    void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current&)
    {
        std::unique_lock lock(dataMutex);
        FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();

        for (; it != tcpTranslationVelocities.end(); it++)
        {

            FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
            FramedDirectionPtr vecOri;
            FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);

            if (itOri != tcpOrientationVelocities.end())
            {
                vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
            }

            const std::string& tcpName = it->first;

            ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);

            if (!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
            {
                offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
            }
            else
            {
                setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
            }

            updateChannel(TCP_TRANS_VELOCITIES_CHANNEL);
            const std::string channelName = tcpName + "Velocities";

            if (!existsChannel(channelName))
            {
                offerChannel(channelName, "pose components of " + tcpName);
                offerDataFieldWithDefault(channelName, "x", Variant(vec->x), "X axis");
                offerDataFieldWithDefault(channelName, "y", Variant(vec->y), "Y axis");
                offerDataFieldWithDefault(channelName, "z", Variant(vec->z), "Z axis");
                offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
                offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
                offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
                offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
            }
            else
            {
                setDataField(channelName, "x", Variant(vec->x));
                setDataField(channelName, "y", Variant(vec->y));
                setDataField(channelName, "z", Variant(vec->z));
                setDataField(channelName, "roll", Variant(vecOri->x));
                setDataField(channelName, "pitch", Variant(vecOri->y));
                setDataField(channelName, "yaw", Variant(vecOri->z));
                setDataField(channelName, "frame", Variant(vec->frame));
            }

            updateChannel(channelName);

        }
    }

}