VisualServoObject.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-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    RobotSkillTemplates::FindAndGraspObjectGroup
 * @author     Valerij Wittenbeck ( valerij dot wittenbeck at student dot kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "VisualServoObject.h"
 
#include <VirtualRobot/Grasping/GraspSet.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/motionmodels/MotionModelAttachedToOtherObject.h>
 
using namespace armarx;
using namespace FindAndGraspObjectGroup;
 
// DO NOT EDIT NEXT LINE
VisualServoObject::SubClassRegistry VisualServoObject::Registry(VisualServoObject::GetName(),
                                                                &VisualServoObject::CreateInstance);
 
VisualServoObject::VisualServoObject(XMLStateConstructorParams stateData) :
    XMLStateTemplate<VisualServoObject>(stateData)
{
}
 
void
VisualServoObject::onEnter()
{
    setLocalObjectChannelFromInput("objectChannel", "objectName"); //still need this?
    setLocalObjectChannelFromInput("markerChannel", "markerName"); //still need this?
 
    //NEW: grasp definition
    // get grasp definition - Taken from GraspingWithVisualFeedback
    FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
    memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx =
        context->priorMemoryProxy->getObjectClassesSegment();
    memoryx::CommonStorageInterfacePrx databasePrx = context->priorMemoryProxy->getCommonStorage();
    memoryx::GridFileManagerPtr fileManager(new memoryx::GridFileManager(databasePrx));
    memoryx::EntityBasePtr entity =
        classesSegmentPrx->getEntityByName(getInput<std::string>("objectName"));
    memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(entity);
    memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
    VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
 
    //    // TODO: remove when grasp definitions can be saved/edited in MemoryX
    //    VirtualRobot::ManipulationObjectPtr mo;
 
    //    std::string armarx_home;
    //    if(std::getenv("ArmarXHome_DIR")){
    //        armarx_home = std::getenv("ArmarXHome_DIR");
    //        mo = VirtualRobot::ObjectIO::loadManipulationObject(armarx_home + "Armar3/data/scene/Vitalis.xml");
    //    }
    //    else
    //        ARMARX_LOG << eERROR << "Problem with ManipulationObject file for StatechartVisualServoingExample!" << flush;
 
 
    //TODO: Get graspSetName and graspName from the configFile! (in order to try various grasps, in case we chose an unreachable one...)
 
    std::string graspSetName = getInput<std::string>("graspSetName");
    VirtualRobot::GraspSetPtr gs = mo->getGraspSet(graspSetName);
 
    if (gs)
    {
        std::string graspName = getInput<std::string>("graspName");
        context->graspDefinition = gs->getGrasp(graspName);
        ARMARX_DEBUG << "VisualServoObject: getting grasp with name: " << graspName << flush;
 
        if (!context->graspDefinition)
        {
            ARMARX_WARNING << "No grasp with name " << graspName << " found! " << flush;
        }
    }
    else
    {
        ARMARX_WARNING << "No grasp set with name " << graspSetName << " found! " << flush;
    }
 
 
    ARMARX_VERBOSE << "Done VisualServoObject::onEnter()" << flush;
}
 
void
VisualServoObject::onExit()
{
    FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
    context->objectMemoryObserver->releaseObjectClass(
        getLocal<ChannelRef>("objectChannel")); //which of these do we still need?
    context->objectMemoryObserver->releaseObjectClass(getLocal<ChannelRef>("markerChannel"));
}
 
void
VisualServoObject::setLocalObjectChannelFromInput(const std::string& objectChannelName,
                                                  const std::string& inputName)
{
    FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
    std::string objectName = getInput<std::string>(inputName);
    ChannelRefBasePtr objectChannel =
        context->objectMemoryObserver->requestObjectClassRepeated(objectName, 30);
 
    if (objectChannel)
    {
        setLocal(objectChannelName, objectChannel);
        ARMARX_DEBUG << "setLocal() for objectName: " << objectName
                     << "; objectChannelName: " << objectChannelName;
    }
    else
    {
        ARMARX_IMPORTANT << "Unknown Object Class: " << objectName;
    }
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
VisualServoObject::GetName()
{
    return "VisualServoObject";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
VisualServoObject::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new VisualServoObject(stateData));
}