GraspCandidateObserver.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 as
* published by the Free Software Foundation; either version 2 of
* the License, or (at your option) any later version.
*
* 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 Lesser 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    RobotAPI
* @author     Simon Ottenhaus
* @copyright  2019 Humanoids Group, H2T, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "GraspCandidateObserver.h"
 
//#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
 
#include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
 
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
using namespace armarx;
using namespace armarx::grasping;
 
GraspCandidateObserver::GraspCandidateObserver()
{
}
 
void
GraspCandidateObserver::onInitObserver()
{
    usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
    offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
}
 
void
GraspCandidateObserver::onConnectObserver()
{
    configTopic = getTopic<GraspCandidateProviderInterfacePrx>(
        getProperty<std::string>("ConfigTopicName").getValue());
    graspCandidateWriter.connect(memoryNameSystem());
}
 
PropertyDefinitionsPtr
GraspCandidateObserver::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(
        new GraspCandidateObserverPropertyDefinitions(getConfigIdentifier()));
}
 
bool
GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter,
                                      const std::string& providerName,
                                      const GraspCandidatePtr& candidate)
{
    if (filter->providerName != "*" && filter->providerName != providerName)
    {
        return false;
    }
    if (filter->minimumSuccessProbability > candidate->graspSuccessProbability)
    {
        return false;
    }
    if (filter->approach != AnyApproach &&
        (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
    {
        return false;
    }
    if (filter->preshape != AnyAperture &&
        (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
    {
        return false;
    }
    if (filter->objectType != objpose::AnyObject && filter->objectType != candidate->objectType)
    {
        return false;
    }
    return true;
}
 
std::string
GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
{
    switch (type)
    {
        case objpose::AnyObject:
            return "AnyObject";
        case objpose::KnownObject:
            return "KnownObject";
        case objpose::UnknownObject:
            return "UnknownObject";
        default:
            return "ERROR";
    }
}
 
void
GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
{
    if (updateCounters.count(providerName) == 0)
    {
        updateCounters[providerName] = 0;
    }
    updateCounters[providerName]++;
    if (providers.count(providerName) == 0)
    {
        providers[providerName] = new ProviderInfo();
    }
 
    if (!existsChannel(providerName))
    {
        offerChannel(providerName, "Channel of " + providerName);
    }
    offerOrUpdateDataField(providerName,
                           "updateCounter",
                           Variant(updateCounters[providerName]),
                           "Counter that increases for each update");
    offerOrUpdateDataField(
        providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
}
 
void
GraspCandidateObserver::reportGraspCandidates(const std::string& providerName,
                                              const GraspCandidateSeq& candidates,
                                              const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    this->candidates[providerName] = candidates;
    graspCandidateWriter.commitGraspCandidateSeq(
        candidates, armarx::armem::Time::Now(), providerName);
    handleProviderUpdate(providerName, candidates.size());
}
 
void
GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName,
                                                      const BimanualGraspCandidateSeq& candidates,
                                                      const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    this->bimanualCandidates[providerName] = candidates;
    handleProviderUpdate(providerName, candidates.size());
}
 
void
GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
                                           const ProviderInfoPtr& info,
                                           const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    providers[providerName] = info;
    if (updateCounters.count(providerName) == 0)
    {
        updateCounters[providerName] = 0;
    }
 
 
    if (!existsChannel(providerName))
    {
        offerChannel(providerName, "Channel of " + providerName);
    }
    offerOrUpdateDataField(providerName, "objectType", ObjectTypeToString(info->objectType), "");
}
 
InfoMap
GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    return providers;
}
 
StringSeq
GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    return getAvailableProviderNames();
}
 
ProviderInfoPtr
GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    checkHasProvider(providerName);
    return providers[providerName];
}
 
bool
GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
{
    std::unique_lock lock(dataMutex);
    return hasProvider(providerName);
}
 
GraspCandidateSeq
GraspCandidateObserver::getAllCandidates(const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    GraspCandidateSeq all;
    for (const auto& pair : candidates)
    {
        all.insert(all.end(), pair.second.begin(), pair.second.end());
    }
    return all;
}
 
GraspCandidateSeq
GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName,
                                                const Ice::Current& c)
{
    return getCandidatesByProviders(Ice::StringSeq{providerName});
}
 
GraspCandidateSeq
GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames,
                                                 const Ice::Current& c)
{
    std::unique_lock lock(dataMutex);
    GraspCandidateSeq all;
    for (const auto& pr : providerNames)
    {
        const auto it = candidates.find(pr);
        if (it != candidates.end())
        {
            all.insert(all.end(), it->second.begin(), it->second.end());
        }
    }
    return all;
}
 
GraspCandidateSeq
GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter,
                                              const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    GraspCandidateSeq matching;
    for (const auto& pair : candidates)
    {
        for (const grasping::GraspCandidatePtr& candidate : pair.second)
        {
            if (FilterMatches(filter, pair.first, candidate))
            {
                matching.push_back(candidate);
            }
        }
    }
    return matching;
}
 
Ice::Int
GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName,
                                                   const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    checkHasProvider(providerName);
    return updateCounters[providerName];
}
 
IntMap
GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
{
    std::unique_lock lock(dataMutex);
    return updateCounters;
}
 
bool
GraspCandidateObserver::setProviderConfig(const std::string& providerName,
                                          const StringVariantBaseMap& config,
                                          const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    if (providers.count(providerName) == 0)
    {
        return false;
    }
    configTopic->setServiceConfig(providerName, config);
    return true;
}
 
void
GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates,
                                              const Ice::Current&)
{
    std::unique_lock lock(selectedCandidatesMutex);
    selectedCandidates = candidates;
}
 
GraspCandidateSeq
GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
{
    std::unique_lock lock(selectedCandidatesMutex);
    return selectedCandidates;
}
 
BimanualGraspCandidateSeq
GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    BimanualGraspCandidateSeq all;
    for (const auto& pair : bimanualCandidates)
    {
        all.insert(all.end(), pair.second.begin(), pair.second.end());
    }
    return all;
}
 
void
GraspCandidateObserver::setSelectedBimanualCandidates(
    const grasping::BimanualGraspCandidateSeq& candidates,
    const Ice::Current&)
{
    std::unique_lock lock(selectedCandidatesMutex);
    selectedBimanualCandidates = candidates;
}
 
BimanualGraspCandidateSeq
GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
{
    std::unique_lock lock(selectedCandidatesMutex);
    return selectedBimanualCandidates;
}
 
void
GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName,
                                                  const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
 
    if (candidates.count(providerName))
    {
        candidates[providerName].clear();
    }
}
 
bool
GraspCandidateObserver::hasProvider(const std::string& providerName)
{
    return providers.count(providerName) > 0;
}
 
void
GraspCandidateObserver::checkHasProvider(const std::string& providerName)
{
    if (!hasProvider(providerName))
    {
        throw LocalException("Unknown provider name '")
            << providerName << "'. Available providers: " << getAvailableProviderNames();
    }
}
 
StringSeq
GraspCandidateObserver::getAvailableProviderNames()
{
    StringSeq names;
    for (const auto& pair : providers)
    {
        names.push_back(pair.first);
    }
    return names;
}