DummyAgentReporter.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-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    MemoryX::ArmarXObjects::DummyAgentReporter
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DummyAgentReporter.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <Eigen/LU>
 
namespace armarx
{
    void DummyAgentReporter::onInitComponent()
    {
        currentVelocity.setZero();
        poseBuffer.reserve(300);
 
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
 
        offeringTopic(getProperty<std::string>("PlatformStateTopicName").getValue());
    }
 
    void DummyAgentReporter::onConnectComponent()
    {
        robotStateComponentPrx = getProxy<armarx::RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
        ARMARX_CHECK_EXPRESSION(robotStateComponentPrx);
 
        memoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
        ARMARX_CHECK_EXPRESSION(memoryPrx);
 
        agentsMemoryPrx = memoryPrx->getAgentInstancesSegment();
        ARMARX_CHECK_EXPRESSION(agentsMemoryPrx);
 
        Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
 
        platformTopic = getTopic<PlatformUnitListenerPrx>(getProperty<std::string>("PlatformStateTopicName").getValue());
 
        robotAgent = new memoryx::AgentInstance();
        auto robot = robotStateComponentPrx->getSynchronizedRobot();
        robotAgent->setSharedRobot(robot);
        robotAgent->setStringifiedSharedRobotInterfaceProxy(iceCommunicator->proxyToString(robot));
        robotAgent->setAgentFilePath(robotStateComponentPrx->getRobotFilename());
 
        Eigen::Matrix3f I = Eigen::Matrix3f::Identity();
        robotOri = new armarx::FramedOrientation(I, armarx::GlobalFrame, "");
        robotPos = new armarx::FramedPosition(Eigen::Vector3f(0, 0, 0), armarx::GlobalFrame, "");
        reportRobotPose();
 
        lastUpdateTime = IceUtil::Time::now();
    }
 
    void DummyAgentReporter::onDisconnectComponent()
    {
    }
 
    void DummyAgentReporter::onExitComponent()
    {
    }
 
    armarx::PropertyDefinitionsPtr DummyAgentReporter::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new DummyAgentReporterPropertyDefinitions(getConfigIdentifier()));
    }
 
    // void armarx::DummyAgentReporter::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current&)
    // {
    //     float x = currentPose.x;
    //     float y = currentPose.y;
    //     float alpha = currentPose.rotationAroundZ;
    //     // needed for Mean_of_circular_quantities
    //     if (alpha > M_PI)
    //     {
    //         alpha = - 2  * M_PI + alpha;
    //     }
 
    //     ARMARX_DEBUG << deactivateSpam(1) << "received new platform pose: " << VAROUT(x) << VAROUT(y) << VAROUT(alpha);
 
    //     {
    //         std::scoped_lock lock(dataMutex);
 
    //         // rotation stored for Mean_of_circular_quantities
    //         // http://en.wikipedia.org/wiki/Mean_of_circular_quantities
    //         Eigen::Vector4d pose2d;
    //         pose2d << x, y, sin(alpha), cos(alpha);
 
    //         Eigen::Matrix3f ori;
    //         ori.setIdentity();
    //         ori = Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitZ());
 
    //         Eigen::Vector3f pos;
    //         pos[0] = x;
    //         pos[1] = y;
    //         pos[2] = 0;
    //         if (alpha < 0)
    //         {
    //             alpha += 2 * M_PI;
    //         }
 
    //         PlatformPose newPose;
    //         newPose.timestampInMicroSeconds = armarx::TimeUtil::GetTime().toMicroSeconds();
    //         newPose.x = x;
    //         newPose.y = y;
    //         newPose.rotationAroundZ = alpha;
    //         // platformTopic->reportPlatformPose(newPose);
 
    //         robotOri = new armarx::FramedOrientation(ori, armarx::GlobalFrame, "");
    //         robotPos = new armarx::FramedPosition(pos, armarx::GlobalFrame, "");
    //     }
 
    //     reportRobotPose();
    // }
 
 
    void armarx::DummyAgentReporter::reportPlatformVelocity(Ice::Float x, Ice::Float y, Ice::Float alpha, const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
 
        currentVelocity << x, y, sin(alpha), cos(alpha) - 1;
        auto delay = IceUtil::Time::now() - lastUpdateTime;
 
        ARMARX_DEBUG << deactivateSpam(1) << "New platform speed received: " << VAROUT(x) << VAROUT(y) << VAROUT(alpha) << " with delay of " << delay.toSecondsDouble();
 
        lastUpdateTime = IceUtil::Time::now();
        velocityDeltaUpdateCount++;
        velocityDeltaSum += delay.toSecondsDouble();
 
        platformTopic->reportPlatformVelocity(x, y, alpha);
    }
 
    void DummyAgentReporter::reportRobotPose()
    {
        std::scoped_lock lock(dataMutex);
 
        if (!robotOri && !robotPos)
        {
            return;
        }
        if (!robotAgent)
        {
            return;
        }
 
        try
        {
            robotAgent->setOrientation(robotOri);
            robotAgent->setPosition(robotPos);
 
            if (robotAgentId.empty() || !agentsMemoryPrx->hasEntityById(robotAgentId))
            {
                robotAgentId = agentsMemoryPrx->addEntity(robotAgent);
                ARMARX_INFO << "Agent created";
            }
            else
            {
                agentsMemoryPrx->updateEntity(robotAgentId, robotAgent);
                ARMARX_INFO << "Agent updated";
            }
        }
        catch (std::exception& e)
        {
            armarx::handleExceptions();
        }
    }
 
 
    void armarx::DummyAgentReporter::reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float alpha, const Ice::Current&)
    {
        platformTopic->reportPlatformOdometryPose(x, y, alpha);
    }
}