GraspSelectionManager.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    RobotComponents::GraspSelectionManager
* @author     Valerij Wittenbeck ( valerij.wittenbeck at student dot kit dot edu)
* @date       2016
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "GraspSelectionManager.h"
 
using namespace armarx;
 
void GraspSelectionManager::onInitComponent()
{
}
 
void GraspSelectionManager::onConnectComponent()
{
}
 
GeneratedGraspList GraspSelectionManager::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
{
    GeneratedGraspList result = grasps;
 
    for (const GraspSelectionCriterionInterfacePrx& criterion : criteria)
    {
        result = criterion->filterGrasps(result);
    }
 
    return result;
}
 
void GraspSelectionManager::registerAsGraspSelectionCriterion(const GraspSelectionCriterionInterfacePrx& criterion, const Ice::Current&)
{
    bool alreadyAdded = std::find_if(criteria.cbegin(), criteria.cend(), [&](const GraspSelectionCriterionInterfacePrx & gsc)
    {
        return gsc->getHash() == criterion->getHash();
    }) != criteria.cend();
 
    if (alreadyAdded)
    {
        ARMARX_ERROR << "criterion '" << criterion->ice_id() << "' already added";
    }
    else
    {
        criteria.push_back(criterion);
    }
}
 
GraspSelectionCriterionInterfaceList GraspSelectionManager::getRegisteredGraspSelectionCriteria(const Ice::Current&)
{
    return criteria;
}
 
GraspingPlacementList
GraspSelectionManager::filterPlacements(const GraspingPlacementList& placements, const Ice::Current&)
{
    ARMARX_INFO << "length of criteria vector: " << criteria.size();
    GraspingPlacementList result = placements;
    for (const GraspSelectionCriterionInterfacePrx& criterion : criteria)
    {
        result = criterion->filterPlacements(result);
    }
    return result;
}