PutAwayLocationPredicateProvider.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    ArmarX::
* @author     Valerij Wittenbeck ( valerij.wittenbeck at kit dot edu)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "PutAwayLocationPredicateProvider.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
using namespace memoryx;
 
PutAwayLocationPredicateProvider::PutAwayLocationPredicateProvider() :
    putAwayLocation(PredicateInfo{"putAwayLocation", 2})
{
}
 
void
PutAwayLocationPredicateProvider::onInitWorldStateUpdater()
{
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
    usingProxy("LongtermMemory");
    usingProxy("GraphNodePoseResolver");
    usingProxy(getProperty<std::string>("CommonPlacesLearnerName").getValue());
 
    cplSegmentName = getProperty<std::string>("CPLSegmentName").getValue();
    sceneName = getProperty<std::string>("PlatformGraphSceneName").getValue();
}
 
void
PutAwayLocationPredicateProvider::onConnectWorldStateUpdater()
{
    getProxy(wm, "WorkingMemory");
    getProxy(prior, "PriorKnowledge");
    getProxy(ltm, "LongtermMemory");
    getProxy(psr, "GraphNodePoseResolver");
    getProxy(cpl, getProperty<std::string>("CommonPlacesLearnerName").getValue());
 
    cpl->setLTMSegmentName(cplSegmentName);
    objectInstances = wm->getObjectInstancesSegment();
    graphSegment = prior->getGraphSegment();
}
 
std::string
PutAwayLocationPredicateProvider::getDefaultName() const
{
    return "PutAwayLocationPredicateProvider";
}
 
PredicateInfoList
PutAwayLocationPredicateProvider::getPredicateInfos(const Ice::Current&)
{
    return {putAwayLocation};
}
 
PredicateInstanceList
PutAwayLocationPredicateProvider::calcPredicates(const Ice::Current&)
{
    PredicateInstanceList result;
    const std::string predicateName = putAwayLocation.name;
 
    for (const auto& entity : objectInstances->getAllEntities())
    {
        auto entry = getCacheEntry(ObjectInstancePtr::dynamicCast(entity)->getMostProbableClass());
        if (entry.putAwayNodeRef)
        {
            result.push_back(PredicateInstance{
                predicateName,
                {objectInstances->getEntityRefById(entity->getId()), entry.putAwayNodeRef},
                true});
        }
    }
 
    return result;
}
 
PutAwayLocationPredicateProvider::CachedCommonPlace
PutAwayLocationPredicateProvider::getCacheEntry(const std::string& className)
{
    auto it = cpCache.find(className);
    if (it != cpCache.end())
    {
        return it->second;
    }
 
    if (cplObjects.empty())
    {
        cplObjectsSegment = ltm->getCustomInstancesSegment(cplSegmentName, false);
        if (!cplObjectsSegment)
        {
            throw armarx::LocalException("No LTM segment named '") << cplSegmentName << "'";
        }
 
        auto entities = cplObjectsSegment->getAllEntities();
        std::transform(entities.cbegin(),
                       entities.cend(),
                       std::back_inserter(cplObjects),
                       [](const EntityBasePtr& e) { return ObjectInstancePtr::dynamicCast(e); });
    }
 
    EntityRefBasePtr nodeRef;
    for (const auto& cplInstance : cplObjects)
    {
        if (cplInstance->getMostProbableClass() == className)
        {
            memoryx::Cluster3DList clusterList =
                cpl->getPositionClustersByComponentCount(cplInstance->getName(), 1);
            if (!clusterList.empty())
            {
                auto cluster = clusterList.front();
                nodeRef = graphSegment->getEntityRefById(psr->getNearestNodeIdToPosition(
                    sceneName, "putdownlocation", cluster.center.x, cluster.center.y));
                break;
            }
        }
    }
 
    if (nodeRef)
    {
        ARMARX_IMPORTANT << "caching putaway location for '" << className << "': '"
                         << nodeRef->entityName << "'";
    }
 
    CachedCommonPlace result{className, nodeRef};
    cpCache.insert({className, result});
    return result;
}