GraspCandidateObserver.h

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
*/
 
#pragma once
 
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
 
#include <mutex>
 
namespace armarx
{
    /**
     * \class GraspCandidateObserverPropertyDefinitions
     * \brief
     */
    class GraspCandidateObserverPropertyDefinitions:
        public ObserverPropertyDefinitions
    {
    public:
        GraspCandidateObserverPropertyDefinitions(std::string prefix):
            ObserverPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("GraspCandidatesTopicName", "GraspCandidatesTopic", "Name of the Grasp Candidate Topic");
            defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigTopic", "Name of the Grasp Candidate Provider Config Topic");
        }
    };
 
    /**
     * \class GraspCandidateObserver
     * \ingroup RobotAPI-SensorActorUnits-observers
     */
    class GraspCandidateObserver :
        virtual public Observer,
        virtual public armarx::armem::ClientPluginUser,
        virtual public grasping::GraspCandidateObserverInterface
    {
    public:
        GraspCandidateObserver();
 
        // framework hooks
        std::string getDefaultName() const override
        {
            return "GraspCandidateObserver";
        }
        void onInitObserver() override;
        void onConnectObserver() override;
 
        /**
         * \see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    public:
        static bool FilterMatches(const grasping::CandidateFilterConditionPtr& filter, const std::string& providerName, const grasping::GraspCandidatePtr& candidate);
        static std::string ObjectTypeToString(objpose::ObjectType type);
 
        // GraspCandidateProviderListener interface
    public:
        void reportProviderInfo(const std::string& providerName, const grasping::ProviderInfoPtr& info, const ::Ice::Current& = Ice::emptyCurrent) override;
        void reportGraspCandidates(const std::string& providerName, const grasping::GraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
        void reportBimanualGraspCandidates(const std::string& providerName, const grasping::BimanualGraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // GraspCandidateObserverInterface interface
    public:
        grasping::InfoMap getAvailableProvidersWithInfo(const ::Ice::Current& = Ice::emptyCurrent) override;
        grasping::StringSeq getAvailableProviderNames(const ::Ice::Current&) override;
        grasping::ProviderInfoPtr getProviderInfo(const std::string& providerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        bool hasProvider(const std::string& providerName, const Ice::Current& c) override;
        grasping::GraspCandidateSeq getAllCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
        grasping::GraspCandidateSeq getCandidatesByProvider(const std::string& providerName, const Ice::Current& c = Ice::emptyCurrent) override;
        grasping::GraspCandidateSeq getCandidatesByProviders(const Ice::StringSeq& providerNames, const Ice::Current& c = Ice::emptyCurrent) override;
        grasping::GraspCandidateSeq getCandidatesByFilter(const grasping::CandidateFilterConditionPtr& filter, const ::Ice::Current& = Ice::emptyCurrent) override;
        Ice::Int getUpdateCounterByProvider(const std::string& providerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        grasping::IntMap getAllUpdateCounters(const Ice::Current& providerName) override;
        bool setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setSelectedCandidates(const grasping::GraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
        grasping::GraspCandidateSeq getSelectedCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // bimanual stuff
        grasping::BimanualGraspCandidateSeq getAllBimanualCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        //        void setSelectedBimanualCandidates(::armarx::grasping::BimanualGraspCandidateSeq, const ::Ice::Current&) = 0;
        void setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
        grasping::BimanualGraspCandidateSeq getSelectedBimanualCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        void clearCandidatesByProvider(const std::string& providerName, const Ice::Current& c) override;
 
    private:
        bool hasProvider(const std::string& providerName);
        void checkHasProvider(const std::string& providerName);
        grasping::StringSeq getAvailableProviderNames();
        std::mutex dataMutex;
        std::map<std::string, grasping::GraspCandidateSeq> candidates;
        std::map<std::string, grasping::BimanualGraspCandidateSeq> bimanualCandidates;
        grasping::InfoMap providers;
        std::map<std::string, int> updateCounters;
 
        grasping::GraspCandidateProviderInterfacePrx configTopic;
 
        std::mutex selectedCandidatesMutex;
        grasping::GraspCandidateSeq selectedCandidates;
 
        grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
        armarx::armem::GraspCandidateWriter graspCandidateWriter;
 
        void handleProviderUpdate(const std::string& providerName, int candidateCount);
    };
 
}