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 <condition_variable>
#include <mutex>
#include <queue>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h>
#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
#include <RobotAPI/components/EarlyVisionGraph/MathTools.h>
#include <RobotAPI/components/units/HeadIKUnit.h>
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/Pose.h>
 
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;
    };
} // namespace armarx