ViewSelection.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::ViewSelection
 * @author     David Schiebener (schiebener at kit dot edu)
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/interface/core/RobotState.h>
 
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
 
#include <RobotAPI/components/units/HeadIKUnit.h>
 
#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
#include <RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h>
#include <RobotAPI/components/EarlyVisionGraph/MathTools.h>
 
 
#include <Eigen/Geometry>
 
 
#include <condition_variable>
#include <mutex>
#include <queue>
 
namespace armarx
{
 
    struct CompareViewTargets
    {
        bool operator()(ViewTargetBasePtr const& t1, ViewTargetBasePtr const& t2)
        {
            if (t1->priority == t2->priority)
            {
                return t1->timeAdded->timestamp < t2->timeAdded->timestamp;
 
            }
            return t1->priority < t2->priority;
        }
    };
 
 
    /**
     * @class ViewSelectionPropertyDefinitions
     * @brief
     */
    class ViewSelectionPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        ViewSelectionPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>("HeadIKUnitName", "HeadIKUnit", "Name of the head IK unit component that should be used");
            defineOptionalProperty<std::string>("HeadIKKinematicChainName", "IKVirtualGaze", "Name of the kinematic chain for the head IK");
            defineOptionalProperty<std::string>("HeadFrameName", "Head Base", "Name of the frame of the head base in the robot model");
            defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
            defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
            defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
            defineOptionalProperty<bool>("VisualizeViewDirection", true, "Draw view ray on DebugLayer.");
            defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
            defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
            defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
            defineOptionalProperty<float>("VisuSaliencyThreshold", 0.0f, "If greater than zero the saliency map is drawn into the debug drawer on each iteration. The value is used as minimum saliency threshold for a point to be shown in debug visu");
        }
    };
 
    /**
     * @defgroup Component-ViewSelection ViewSelection
     * @ingroup RobotAPI-Components
     * @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
     * of the current requested object locations.
     * The uncertainty of objects grow based on their motion model and the timed passed since the last localization.
     * It can be activated or deactivated with the Ice interface and given manual target positions to look at.
     */
 
    /**
     * @ingroup Component-ViewSelection
     * @brief The ViewSelection class
     */
    class ViewSelection :
        virtual public Component,
        virtual public ViewSelectionInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "ViewSelection";
        }
 
    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 addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::emptyCurrent) override;
 
        void clearManualViewTargets(const Ice::Current& c = Ice::emptyCurrent) override;
 
        ViewTargetList getManualViewTargets(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
        {
            std::unique_lock lock(manualViewTargetsMutex);
 
            ARMARX_INFO << "activating automatic view selection";
 
            doAutomaticViewSelection = true;
            viewSelectionObserver->onActivateAutomaticViewSelection();
        }
 
        void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
        {
            std::unique_lock lock(manualViewTargetsMutex);
 
            ARMARX_INFO << "deactivating automatic view selection";
 
            doAutomaticViewSelection = false;
            viewSelectionObserver->onDeactivateAutomaticViewSelection();
        }
 
        bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
        {
            std::unique_lock lock(manualViewTargetsMutex);
 
            return doAutomaticViewSelection;
        }
 
        void updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current& c = Ice::emptyCurrent) override;
 
        void removeSaliencyMap(const std::string& name, const Ice::Current& c = Ice::emptyCurrent) override;
 
        ::Ice::StringSeq getSaliencyMapNames(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c = Ice::emptyCurrent) override;
 
        void clearSaliencySphere(const Ice::Current& c = Ice::emptyCurrent) override;
 
 
    private:
 
        void process();
 
        ViewTargetBasePtr nextAutomaticViewTarget();
 
        void getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps);
 
        armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;
 
        RobotStateComponentInterfacePrx robotStateComponent;
        HeadIKUnitInterfacePrx headIKUnitProxy;
        DebugDrawerInterfacePrx drawer;
 
        ViewSelectionObserverPrx viewSelectionObserver;
 
        std::string headIKKinematicChainName;
        std::string headFrameName;
        std::string cameraFrameName;
 
        CIntensityGraph* visibilityMaskGraph;
 
        float sleepingTimeBetweenViewDirectionChanges;
        IceUtil::Time timeOfLastViewChange;
 
        bool drawViewDirection;
 
        std::mutex manualViewTargetsMutex;
        std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets;
 
        bool doAutomaticViewSelection;
 
        Eigen::Vector3f offsetToHeadCenter;
 
        std::condition_variable condition;
        bool hasNewSaliencyMap;
        std::mutex syncMutex;
 
        std::map<std::string, SaliencyMapBasePtr> saliencyMaps;
 
        float visuSaliencyThreshold;
 
    };
}