KinematicSelfLocalization.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    ArmarXSimulation::ArmarXObjects::KinematicSelfLocalization
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KinematicSelfLocalization.h"
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
 
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/interface/core/GeometryBase.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
 
using namespace armarx;
 
void
KinematicSelfLocalization::onInitComponent()
{
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
 
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    usingTopic(getProperty<std::string>("PlatformTopicName").getValue());
}
 
void
KinematicSelfLocalization::onConnectComponent()
{
    robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(
        getProperty<std::string>("RobotStateComponentName").getValue());
    ARMARX_CHECK_EXPRESSION(robotStateComponentPrx);
    ARMARX_INFO << "Retrieved robot state proxy...";
 
    memoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(
        getProperty<std::string>("WorkingMemoryName").getValue());
    ARMARX_CHECK_EXPRESSION(memoryPrx);
    ARMARX_INFO << "Retrieved WM proxy...";
 
    agentsMemoryPrx = memoryPrx->getAgentInstancesSegment();
    ARMARX_CHECK_EXPRESSION(agentsMemoryPrx);
    ARMARX_INFO << "Retrieved WM agent segment ...";
 
    Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
 
    robotName = getProperty<std::string>("RobotName").getValue();
    robotPoseZ =
        getProperty<float>("RobotPoseZ").isSet() ? getProperty<float>("RobotPoseZ").getValue() : 0;
 
    std::string agentName = getProperty<std::string>("AgentName").isSet()
                                ? getProperty<std::string>("AgentName").getValue()
                                : robotName;
 
 
    ARMARX_INFO << "Creating robot agent with name " << agentName;
    robotAgent = new memoryx::AgentInstance(agentName);
    auto robot = robotStateComponentPrx->getSynchronizedRobot();
    robotAgent->setSharedRobot(robot);
    robotAgent->setName(agentName);
    robotAgent->setStringifiedSharedRobotInterfaceProxy(iceCommunicator->proxyToString(robot));
    robotAgent->setAgentFilePath(robotStateComponentPrx->getRobotFilename());
    ARMARX_INFO << "Creating robot agent...done";
 
    // Simulating platform movement to initial pose
    Eigen::Isometry3f global_T_robot_initial = Eigen::Isometry3f::Identity();
    global_T_robot_initial.translation().x() =
        getProperty<float>("InitialPlatformPoseX").getValue();
    global_T_robot_initial.translation().y() =
        getProperty<float>("InitialPlatformPoseY").getValue();
    global_T_robot_initial.linear() =
        simox::math::rpy_to_mat3f(0, 0, getProperty<float>("InitialPlatformPoseAngle").getValue());
 
    TransformStamped transformStamped;
    transformStamped.transform = global_T_robot_initial.matrix();
    transformStamped.header.agent = agentName;
    transformStamped.header.frame = GlobalFrame;
    transformStamped.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
 
    reportGlobalRobotPose(transformStamped);
 
    // periodic task setup
    cycleTime = 30;
 
    if (execTask)
    {
        execTask->stop();
    }
 
    execTask =
        new PeriodicTask<KinematicSelfLocalization>(this,
                                                    &KinematicSelfLocalization::reportRobotPose,
                                                    cycleTime,
                                                    false,
                                                    "KinematicSelfLocalizationCalculation");
    execTask->start();
    execTask->setDelayWarningTolerance(100);
}
 
void
KinematicSelfLocalization::onDisconnectComponent()
{
    execTask->stop();
}
 
void
KinematicSelfLocalization::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
KinematicSelfLocalization::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new KinematicSelfLocalizationPropertyDefinitions(getConfigIdentifier()));
}
 
void
KinematicSelfLocalization::reportRobotPose()
{
    try
    {
        std::unique_lock lock(dataMutex);
        ARMARX_DEBUG << "Reporting robot agent:" << robotAgent->getName()
                     << ", robotAgentId:" << robotAgentId << ", self:" << this->getName();
 
 
        if (!currentRobotPose)
        {
            return;
        }
        robotAgent->setPose(currentRobotPose);
 
        robotAgentId = agentsMemoryPrx->upsertEntityByName(robotAgent->getName(), robotAgent);
        robotAgent->setId(robotAgentId);
    }
    catch (...)
    {
        ARMARX_WARNING << deactivateSpam(10) << "Could not update robot pose in agents segment...\n"
                       << GetHandledExceptionString();
        robotAgentId.clear();
    }
}
 
void
KinematicSelfLocalization::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped,
                                                 const ::Ice::Current&)
{
    std::unique_lock lock(dataMutex);
 
    Eigen::Isometry3f global_T_robot(transformStamped.transform);
    global_T_robot.translation().z() = robotPoseZ; // TODO lecagy. Is this really necessary?
 
    currentRobotPose = new FramedPose(global_T_robot.matrix(), armarx::GlobalFrame, "");
}