OnTablePredicateProvider.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 "OnTablePredicateProvider.h"
 
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
//#include <VirtualRobot/BoundingBox.h>
 
using namespace memoryx;
 
OnTablePredicateProvider::OnTablePredicateProvider()
{
}
 
 
void OnTablePredicateProvider::onInitWorldStateUpdater()
{
    usingProxy("WorkingMemory");
 
}
 
void OnTablePredicateProvider::onConnectWorldStateUpdater()
{
    workingMemoryProxy = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
    objectInstancesProxy = workingMemoryProxy->getObjectInstancesSegment();
}
 
std::string OnTablePredicateProvider::getDefaultName() const
{
    return "OnTablePredicateProvider";
}
 
PredicateInfo OnTablePredicateProvider::getPredicateInfo(const Ice::Current&)
{
    return PredicateInfo {"ontable", 1};
}
 
PredicateInstanceList OnTablePredicateProvider::calcPredicates(const Ice::Current&)
{
    PredicateInstanceList returnPIList;
 
    // TODO: only for testing
    //    Ice::StringSeq objs{"nesquik", "table"};
    //    PredicateInstance pI = {"on", Ice::StringSeq{"nesquik", "table"} };
    //returnPIList.push_back(PredicateInstance{"on", Ice::StringSeq{"nesquik", "table"} } );
 
    struct TableDef
    {
        std::string name;
        Eigen::Vector3f min;
        Eigen::Vector3f max;
    };
 
    std::vector<TableDef> tables;
    tables.push_back(TableDef {"table1", Eigen::Vector3f(-600,  600, 900), Eigen::Vector3f(600, 1500, 1400)});
    tables.push_back(TableDef {"table2", Eigen::Vector3f(-600, -1500, 900), Eigen::Vector3f(600, -600, 1400)});
 
 
    // get all currently localized objects and their positions from the working memory
    EntityIdList objectIds = objectInstancesProxy->getAllEntityIds();
 
    for (std::string id : objectIds)
    {
        EntityRefBasePtr entityBase = objectInstancesProxy->getEntityRefById(id);
        const ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entityBase->getEntity());
 
        if (!object)
        {
            ARMARX_WARNING << "Something went wrong when getting the object with id " << id << " from working memory";
        }
        else
        {
            for (TableDef table : tables)
            {
                // TODO: assert that positions are in base coordinate frame
                auto pos = object->getPosition();
 
                //                assert(pos->getFrame() == "Armar3_Base");
                if (pos->x > table.min[0] && pos->y > table.min[1] && pos->z > table. min[2]
                    && pos->x < table.max[0] && pos->y < table.max[1] && pos->z < table.max[2])
                {
                    //                    returnPIList.push_back(PredicateInstance{"on", Ice::StringSeq{object->getName(), table.name}, true});
                    //@TODO
                }
            }
        }
    }
 
    /*EntityIdList objectIds = objectInstancesProxy->getAllEntityIds();
    for(EntityIdList::const_iterator it = objectIds.begin(); it != objectIds.end(); it++)
    {
        EntityRefBasePtr entityBase = objectInstancesProxy->getEntityRefById(*it);
        const ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entityBase->getEntity());
        if (object)
        {
            // TODO: assert that positions are in base coordinate frame
            auto pos = object->getPosition();
    //                assert(pos->getFrame() == "Armar3_Base");
            if(pos->x > min[0] && pos->y > min[1] && pos->z > min[2] && pos->x < max[0] && pos->y < max[1] && pos->z < max[2])
                returnPIList.push_back(PredicateInstance{"on", Ice::StringSeq{object->getName(), "table"}});
        }
        else
        {
            ARMARX_WARNING << "Something went wrong when getting the object with id " << *it << " from working memory";
        }
    }*/
 
 
 
    return returnPIList;
}