OnTopPredicateProvider.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     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "OnTopPredicateProvider.h"
 
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
 
using namespace memoryx ;
 
OnTopPredicateProvider::OnTopPredicateProvider()
{
}
 
 
 
void memoryx::OnTopPredicateProvider::onInitWorldStateUpdater()
{
    usingProxy("WorkingMemory");
 
}
 
void memoryx::OnTopPredicateProvider::onConnectWorldStateUpdater()
{
    workingMemoryProxy = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
    objectInstancesProxy = workingMemoryProxy->getObjectInstancesSegment();
}
 
std::string memoryx::OnTopPredicateProvider::getDefaultName() const
{
    return "OnTopPredicateProvider";
}
 
PredicateInfo memoryx::OnTopPredicateProvider::getPredicateInfo(const Ice::Current&)
{
    return PredicateInfo {"ontop", 2};
}
 
 
PredicateInstanceList memoryx::OnTopPredicateProvider::calcPredicates(const Ice::Current&)
{
    PredicateInstanceList returnPIList;
 
    const float maxDistanceXY = 50;
    const float minDistanceZ = 40;
    const float maxDistanceZ = 200;
 
    // get all currently localized objects and their positions from the working memory
    std::map<std::string, armarx::FramedPositionPtr> objectPositions;
    EntityIdList objectIds = objectInstancesProxy->getAllEntityIds();
 
    for (EntityIdList::const_iterator it = objectIds.begin(); it != objectIds.end(); it++)
    {
        const EntityBasePtr entityBase = objectInstancesProxy->getEntityById(*it);
        const ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entityBase);
 
        if (object)
        {
            // TODO: assert that positions are in base coordinate frame
            objectPositions[object->getName()] = object->getPosition();
        }
        else
        {
            ARMARX_WARNING << "Something went wrong when getting the object with id " << *it << " from working memory";
        }
    }
 
    // determine which object pairs are standing on one another
    for (std::map<std::string, armarx::FramedPositionPtr>::iterator i = objectPositions.begin(); i != objectPositions.end(); i++)
    {
        Eigen::Vector3f pos1 = i->second->toEigen();
        ARMARX_IMPORTANT << "Object " << i->first << " at position " << i->second->toEigen();
        std::map<std::string, armarx::FramedPositionPtr>::iterator j = i;
        j++;
 
        for (; j != objectPositions.end(); j++)
        {
            Eigen::Vector3f pos2 = j->second->toEigen();
 
            // objects have to be close to each other in the x-y-plane
            if (sqrtf((pos1(0) - pos2(0)) * (pos1(0) - pos2(0)) + (pos1(1) - pos2(1)) * (pos1(1) - pos2(1))) < maxDistanceXY)
            {
                // difference in z coordinate must be between min and max value
                if (fabs(pos1(2) - pos2(2)) > minDistanceZ &&  fabs(pos1(2) - pos2(2)) < maxDistanceZ)
                {
                    if (pos1(2) > pos2(2))
                    {
                        //@TODO
                        //                        returnPIList.push_back(PredicateInstance{"on", Ice::StringSeq{i->first, j->first}, true});
                    }
                    else
                    {
                        //                        returnPIList.push_back(PredicateInstance{"on", Ice::StringSeq{j->first, i->first}, true});
                    }
                }
            }
        }
    }
 
    return returnPIList;
}