AffordanceExtraction.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 Lesser 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    VisionX
 * @author     Peter Kaiser (peter dot kaiser at kit dot edu)
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <ArmarXCore/interface/observers/ObserverInterface.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
 
#include <VisionX/interface/components/PointCloudSegmenter.h>
#include <VisionX/interface/components/AffordanceExtraction.h>
 
#include <AffordanceKit/affordances/PlatformGraspAffordance.h>
#include <AffordanceKit/affordances/PrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/GraspAffordance.h>
#include <AffordanceKit/affordances/SupportAffordance.h>
#include <AffordanceKit/affordances/LeanAffordance.h>
#include <AffordanceKit/affordances/HoldAffordance.h>
#include <AffordanceKit/affordances/LiftAffordance.h>
#include <AffordanceKit/affordances/TurnAffordance.h>
#include <AffordanceKit/affordances/PushAffordance.h>
#include <AffordanceKit/affordances/PullAffordance.h>
#include <AffordanceKit/affordances/BimanualPlatformGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualPrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualOpposedGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualAlignedGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualOpposedPlatformGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualOpposedPrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualAlignedPlatformGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualAlignedPrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualTurnAffordance.h>
#include <AffordanceKit/affordances/BimanualLiftAffordance.h>
 
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/core/memory/SegmentedMemory.h>
#include <MemoryX/libraries/memorytypes/segment/AffordanceSegment.h>
#include <MemoryX/libraries/memorytypes/segment/EnvironmentalPrimitiveSegment.h>
 
#include <Eigen/Eigen>
#include <mutex>
 
namespace armarx
{
    /**
     * @class AffordanceExtractionPropertyDefinitions
     * @brief
     */
    class AffordanceExtractionPropertyDefinitions:
        public armarx::ComponentPropertyDefinitions
    {
    public:
        AffordanceExtractionPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
            defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory component");
            defineOptionalProperty<std::string>("AffordanceExtractionTopicName", "ExtractedAffordances", "Topic for publishing extraction notifications");
            defineOptionalProperty<std::string>("PrimitiveExtractionTopicName", "SegmentedPointCloud", "Topic under which primitive extraction notifications are published");
            defineOptionalProperty<std::string>("EmbodimentName", "WalkMan", "Name of the embodiment to use for affordance extraction (Armar3, Armar4, WalkMan");
            defineOptionalProperty<float>("SpatialSamplingDistance", 75, "Distance between two spatial samplings in mm");
            defineOptionalProperty<int>("NumOrientationalSamplings", 4, "Number of orientational samplings");
            defineOptionalProperty<std::string>("RestrictToAffordanceList", "", "Only evaluate affordances from this list (use short names in comma separated list)");
            defineOptionalProperty<int>("MaxPrimitiveSamplingSize", 0, "Maximum size of primitive sampling to be considered for extracting affordances (0 means infinity)");
            defineOptionalProperty<float>("AffordanceExtractionCertainty", 0.8, "Certainty value attributed to visual affordance extraction");
        }
    };
 
    /**
     * @class AffordanceExtraction
     * @ingroup RobotSkillTemplates-Components
     * @brief A brief description
     *
     * Detailed Description
     */
    class AffordanceExtraction :
        virtual public Component,
        virtual public AffordanceExtractionInterface
    {
    public:
        AffordanceExtraction();
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "AffordanceExtraction";
        }
 
        void reportNewPointCloudSegmentation(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void enablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
        void disablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
        bool isPipelineStepEnabled(const Ice::Current& c = Ice::emptyCurrent) override;
        armarx::TimestampBasePtr getLastProcessedTimestamp(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void addObservation(const PoseBasePtr& endEffectorPose, const std::string& affordance, const std::string& primitiveId, float result, float certainty, float sigma_pos, float sigma_rot, const Ice::Current& c = Ice::emptyCurrent) override;
 
        void exportScene(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
        void importScene(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        void affordanceExtractionThread();
 
    protected:
        AffordanceExtractionListenerPrx listenerPrx;
 
        std::mutex affordancesMutex;
 
        std::mutex primitivesMutex;
        bool hasNewPrimitives;
 
        std::mutex timestampMutex;
        long lastProcessedTimestamp;
 
        std::mutex enableMutex;
        bool enabled;
 
        PeriodicTask<AffordanceExtraction>::pointer_type affordanceExtractionTask;
 
        memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
        memoryx::AffordanceSegmentBasePrx affordanceSegment;
        memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment;
 
        AffordanceKit::PrimitiveSetPtr primitiveSegmentation;
 
        AffordanceKit::EmbodimentPtr embodiment;
 
        AffordanceKit::PlatformGraspAffordancePtr platformGraspAffordance;
        AffordanceKit::PrismaticGraspAffordancePtr prismaticGraspAffordance;
        AffordanceKit::GraspAffordancePtr graspAffordance;
 
        AffordanceKit::SupportAffordancePtr supportAffordance;
        AffordanceKit::LeanAffordancePtr leanAffordance;
        AffordanceKit::HoldAffordancePtr holdAffordance;
        AffordanceKit::LiftAffordancePtr liftAffordance;
        AffordanceKit::TurnAffordancePtr turnAffordance;
        AffordanceKit::PushAffordancePtr pushAffordance;
        AffordanceKit::PullAffordancePtr pullAffordance;
 
        AffordanceKit::BimanualPlatformGraspAffordancePtr bimanualPlatformGraspAffordance;
        AffordanceKit::BimanualPrismaticGraspAffordancePtr bimanualPrismaticGraspAffordance;
        AffordanceKit::BimanualGraspAffordancePtr bimanualGraspAffordance;
        AffordanceKit::BimanualOpposedGraspAffordancePtr bimanualOpposedGraspAffordance;
        AffordanceKit::BimanualAlignedGraspAffordancePtr bimanualAlignedGraspAffordance;
        AffordanceKit::BimanualOpposedPlatformGraspAffordancePtr bimanualOpposedPlatformGraspAffordance;
        AffordanceKit::BimanualOpposedPrismaticGraspAffordancePtr bimanualOpposedPrismaticGraspAffordance;
        AffordanceKit::BimanualAlignedPlatformGraspAffordancePtr bimanualAlignedPlatformGraspAffordance;
        AffordanceKit::BimanualAlignedPrismaticGraspAffordancePtr bimanualAlignedPrismaticGraspAffordance;
        AffordanceKit::BimanualTurnAffordancePtr bimanualTurnAffordance;
        AffordanceKit::BimanualLiftAffordancePtr bimanualLiftAffordance;
 
        std::map<std::string, memoryx::AffordanceType> affordanceTypes;
 
        std::vector<AffordanceKit::UnimanualAffordancePtr> unimanualAffordances;
        std::vector<AffordanceKit::BimanualAffordancePtr> bimanualAffordances;
 
        std::set<std::string> affordanceRestriction;
 
        unsigned int maxPrimitiveSamplingSize;
        float affordanceExtractionCertainty;
 
 
        DebugObserverInterfacePrx debugObserver;
    };
}