SceneModifier.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 "SceneModifier.h"
 
#include <signal.h>
 
#include <Eigen/Core>
 
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
#include <ArmarXCore/observers/condition/Term.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
using namespace armarx;
using namespace memoryx;
 
void
SceneModifier::onInitComponent()
{
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
    usingProxy(getProperty<std::string>("SimulatorName").getValue());
 
    if (getProperty<bool>("LocalizeObject").getValue())
    {
        usingProxy("ObjectMemoryObserver");
        usingProxy("ConditionHandler");
    }
}
 
void
SceneModifier::onExitComponent()
{
    ARMARX_INFO << "Exiting SceneModifier";
    // how do we terminate the session correctly?
    //this->getArmarXManager()->shutdown();
    //raise(SIGTERM);
}
 
void
SceneModifier::onDisconnectComponent()
{
    if (classChannel1 && memoryObserver)
    {
        ARMARX_INFO << "Releasing obj class ";
        memoryObserver->releaseObjectClass(classChannel1);
        classChannel1 = NULL;
    }
}
 
bool
SceneModifier::checkForConfigs(const std::string& suffix)
{
    std::stringstream strStr;
 
    strStr << "SceneModificationType";
 
    if (!suffix.empty())
    {
        strStr << suffix;
    }
 
    ARMARX_DEBUG << "Checking for property " << strStr.str() << "...";
 
    ///////////////// add/move/remove objects
    std::string mode;
 
    try
    {
        mode = getProperty<std::string>(strStr.str()).getValue();
    }
    catch (...)
    {
        ARMARX_DEBUG << "Failed to get property " << strStr.str() << "...";
        return false;
    }
 
    if (mode.empty())
    {
        return false;
    }
 
    if (mode == "addObject")
    {
        addObject(suffix);
    }
    else if (mode == "removeObject")
    {
        removeObject(suffix);
    }
    else if (mode == "moveObject")
    {
        moveObject(suffix);
    }
    else if (mode != "none")
    {
        ARMARX_ERROR << "Unknown mode: " << mode << std::endl;
    }
 
    return true;
}
 
void
SceneModifier::onConnectComponent()
{
    ARMARX_INFO << "Starting SceneModifier";
 
    workingMemoryProxy = getProxy<memoryx::WorkingMemoryInterfacePrx>(
        getProperty<std::string>("WorkingMemoryName").getValue());
    priorKnowledgeProxy = getProxy<memoryx::PriorKnowledgeInterfacePrx>(
        getProperty<std::string>("PriorKnowledgeName").getValue());
    simulatorPrx =
        getProxy<SimulatorInterfacePrx>(getProperty<std::string>("SimulatorName").getValue());
 
    checkForConfigs("");
 
    for (int i = 0; i < 10; i++)
    {
        std::stringstream strStr;
        strStr << "_" << i;
        checkForConfigs(strStr.str());
    }
 
 
    ////////////// start object localizer
    if (getProperty<bool>("LocalizeObject").getValue())
    {
        std::string objClassName = getProperty<std::string>("ObjectClassName").getValue();
        ARMARX_VERBOSE << "localizing object " << objClassName;
 
        // retrieve proxies
        memoryObserver =
            getProxy<memoryx::ObjectMemoryObserverInterfacePrx>("ObjectMemoryObserver");
        conditionHandler = getProxy<ConditionHandlerInterfacePrx>("ConditionHandler");
        int timeS = getProperty<int>("LocalizeObject_seconds").getValue();
        localizeSingleObject(objClassName, timeS);
    }
 
 
    /////////////// modify robot
    std::string attachObjectName =
        getProperty<std::string>("AttachObjectToRobot.ObjectName").getValue();
    std::string attachRobotName =
        getProperty<std::string>("AttachObjectToRobot.RobotName").getValue();
    std::string attachRobotNodeName =
        getProperty<std::string>("AttachObjectToRobot.RobotNodeName").getValue();
 
    if (!attachObjectName.empty() && !attachRobotName.empty() && !attachRobotNodeName.empty())
    {
        ARMARX_VERBOSE << "Attaching " << attachObjectName << "to robot " << attachRobotName
                       << " at robot node " << attachRobotNodeName;
        attachObject(attachObjectName, attachRobotName, attachRobotNodeName);
    }
 
 
    // that's it, we can shut down
    // todo: how to shutdown correctly?!
    //this->terminate();
    //raise(SIGTERM);
}
 
Eigen::Matrix4f
SceneModifier::getGlobalPose(const std::string& suffix)
{
    std::stringstream ss1, ss2, ss3, ss4, ss5, ss6;
    ss1 << "PositionX";
    ss2 << "PositionY";
    ss3 << "PositionZ";
    ss4 << "OrientationRoll";
    ss5 << "OrientationPitch";
    ss6 << "OrientationYaw";
 
    if (!suffix.empty())
    {
        ss1 << suffix;
        ss2 << suffix;
        ss3 << suffix;
        ss4 << suffix;
        ss5 << suffix;
        ss6 << suffix;
    }
 
    float x = getProperty<float>(ss1.str()).getValue();
    float y = getProperty<float>(ss2.str()).getValue();
    float z = getProperty<float>(ss3.str()).getValue();
    float ro = getProperty<float>(ss4.str()).getValue();
    float pi = getProperty<float>(ss5.str()).getValue();
    float ya = getProperty<float>(ss6.str()).getValue();
 
    Eigen::Vector3f pos = {x, y, z};
    Eigen::Matrix4f globalPose;
    VirtualRobot::MathTools::rpy2eigen4f(ro, pi, ya, globalPose);
    globalPose.block<3, 1>(0, 3) = pos;
    return globalPose;
}
 
bool
SceneModifier::getStatic(const std::string& suffix)
{
    std::stringstream ss1;
    ss1 << "StaticObject";
 
 
    if (!suffix.empty())
    {
        ss1 << suffix;
    }
 
    bool x = getProperty<bool>(ss1.str()).getValue();
 
    return x;
}
 
void
SceneModifier::attachObject(const std::string& objName,
                            const std::string& robotName,
                            const std::string& robotNodeName)
{
    if (!simulatorPrx->hasObject(objName))
    {
        ARMARX_ERROR << "Object " << objName << " is not present in simulator...";
        return;
    }
 
    if (!simulatorPrx->hasRobot(robotName))
    {
        ARMARX_ERROR << "Robot " << robotName << " is not present in simulator...";
        return;
    }
 
    if (!simulatorPrx->hasRobotNode(robotName, robotNodeName))
    {
        ARMARX_ERROR << "Robot node " << robotNodeName << " is not present in simulator...";
        return;
    }
 
    simulatorPrx->objectGrasped(robotName, robotNodeName, objName);
}
 
void
SceneModifier::addObject(const std::string& suffix)
{
    std::stringstream ss1, ss2;
    ss1 << "ObjectClassName";
    ss2 << "ObjectInstanceName";
 
    if (!suffix.empty())
    {
        ss1 << suffix;
        ss2 << suffix;
    }
 
    ARMARX_DEBUG << "checking for object class property: " << ss1.str()
                 << ", and instance name:" << ss2.str();
 
    std::string objClassName = getProperty<std::string>(ss1.str()).getValue();
    std::string objInstanceName = getProperty<std::string>(ss2.str()).getValue();
    ARMARX_DEBUG << "adding object class " << objClassName << ", instance name:" << objInstanceName;
 
 
    memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx =
        priorKnowledgeProxy->getObjectClassesSegment();
 
    //memoryx::EntityIdList list = classesSegmentPrx->getAllEntityIds();
    //for (size_t i=0;i<list.size();i++)
    //{
    //    memoryx::EntityPtr e = memoryx::EntityPtr::dynamicCast(classesSegmentPrx->getEntityById(list.at(i)));
    //    ARMARX_IMPORTANT_S << "i:" << i << ", id:" << e->getId() << ", name:" << e->getName();
    //}
    memoryx::EntityBasePtr classesEntity = classesSegmentPrx->getEntityByName(objClassName);
 
    if (!classesEntity)
    {
        ARMARX_ERROR << "No memory entity found with class name " << objClassName;
        return;
    }
 
    memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(classesEntity);
 
    if (!objectClass)
    {
        ARMARX_ERROR << "Could not cast entitiy to object class, name: " << objClassName;
        return;
    }
 
    Eigen::Matrix4f globalPose = getGlobalPose(suffix);
    PosePtr pose = new Pose(globalPose);
 
    bool isStatic = getStatic(suffix);
 
    if (simulatorPrx->hasObject(objInstanceName))
    {
        ARMARX_ERROR << "Object " << objInstanceName << " is already present...";
        return;
    }
 
    ARMARX_IMPORTANT << "Adding Object " << objInstanceName << " at :\n" << pose;
    simulatorPrx->addObject(objectClass, objInstanceName, pose, isStatic);
}
 
void
SceneModifier::removeObject(const std::string& suffix)
{
    /*std::stringstream ss1,ss2;
    ss1 << "ObjectClassName";
    ss2 << "ObjectInstanceName";
    if (!suffix.empty())
    {
        ss1 << _ << suffix;
        ss2 << _ << suffix;
    }
    std::string objClassName = getProperty<std::string>(ss1).getValue();
    std::string objInstanceName = getProperty<std::string>(ss2).getValue();*/
 
    ARMARX_WARNING << " NYI....";
}
 
void
SceneModifier::moveObject(const std::string& suffix)
{
    //std::string objClassName = getProperty<std::string>("ObjectClassName").getValue();
    std::stringstream ss1, ss2;
    ss1 << "ObjectClassName";
    ss2 << "ObjectInstanceName";
 
    if (!suffix.empty())
    {
        ss1 << suffix;
        ss2 << suffix;
    }
 
    std::string objInstanceName = getProperty<std::string>(ss2.str()).getValue();
    ARMARX_DEBUG << "moving object instance :" << objInstanceName;
 
    Eigen::Matrix4f globalPose = getGlobalPose(suffix);
    PosePtr pose = new Pose(globalPose);
 
    if (!simulatorPrx->hasObject(objInstanceName))
    {
        ARMARX_ERROR << "Object " << objInstanceName << " is not present in simulator...";
        return;
    }
 
    ARMARX_VERBOSE << "Moving object " << objInstanceName << " to :\n" << pose;
    simulatorPrx->setObjectPose(objInstanceName, pose);
}
 
void
SceneModifier::localizeSingleObject(const std::string& objClass, int sleepS)
{
    /////////////////////////////////////////////////////////////////////////////////////////////////
    // USECASE I: query a specific object and wait until localization finished
    /////////////////////////////////////////////////////////////////////////////////////////////////
 
    ARMARX_VERBOSE << "Initiating localization of " << objClass << " for " << sleepS << " seconds";
 
    // 1. request the object class (corresponding to the classes in prior knowledge see PriorMemoryEditorGui)
    classChannel1 = memoryObserver->requestObjectClassRepeated(
        objClass, 50, armarx::DEFAULT_VIEWTARGET_PRIORITY);
 
    if (!classChannel1)
    {
        ARMARX_ERROR << "Could not find class " << objClass << " in prior knowledge";
        return;
    }
 
 
    // 2. install condition on the localizationFinished datafield
    Literal objectLocalizationFinished1(
        ChannelRefPtr::dynamicCast(classChannel1)->getDataFieldIdentifier("localizationFinished"),
        "equals",
        Literal::createParameterList(true));
    EventBasePtr e1 = new EventBase();
    ConditionIdentifier id1 =
        conditionHandler->installCondition(EventListenerInterfacePrx::uncheckedCast(getProxy()),
                                           objectLocalizationFinished1.getImpl(),
                                           e1,
                                           true,
                                           false,
                                           {});
 
    ARMARX_VERBOSE << "Sleeping " << sleepS << " seconds";
    TimeUtil::MSSleep(sleepS * 1000);
 
    // 3. fetch object instances for the requested object class
    ChannelRefBaseSequence channelRefs = memoryObserver->getObjectInstances(classChannel1);
    ChannelRefBaseSequence::iterator iter = channelRefs.begin();
    ARMARX_INFO_S << "Found the following instance:";
 
    while (iter != channelRefs.end())
    {
        ChannelRefPtr channelRef = ChannelRefPtr::dynamicCast(*iter);
        ARMARX_INFO << channelRef->getDataField("instanceName");
        iter++;
    }
 
    // 4. release object class (do not forget)
    memoryObserver->releaseObjectClass(classChannel1);
    classChannel1 = NULL;
 
    conditionHandler->removeCondition(id1);
}