/* * 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 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) * @date 2015 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt * GNU General Public License */ #ifndef _ARMARX_COMPONENT_RobotComponents_ViewSelection_H #define _ARMARX_COMPONENT_RobotComponents_ViewSelection_H #include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/services/tasks/RunningTask.h> #include <RobotAPI/libraries/core/Pose.h> #include <RobotAPI/interface/core/RobotState.h> #include <RobotAPI/components/units/HeadIKUnit.h> #include <MemoryX/interface/workingmemory/AbstractWorkingMemoryInterface.h> #include <MemoryX/interface/memorytypes/MemorySegments.h> #include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h> #include <MemoryX/libraries/memorytypes/entity/ObjectClass.h> #include <RobotComponents/interface/components/ViewSelectionInterface.h> #include <Eigen/Geometry> // graph stuff #include "EarlyVisionGraph/IntensityGraph.h" #include "EarlyVisionGraph/GraphPyramidLookupTable.h" #include "EarlyVisionGraph/MathTools.h" //#include "EarlyVisionGraph/GraphTriangulation.h" //#include "EarlyVisionGraph/GraphGenerator.h" namespace armarx { /** * @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>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component that should be used"); defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge 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<float>("MaxObjectDistance", 1500.f, "Maximum distance of objects the head to be considered for observation"); defineOptionalProperty<float>("HalfCameraOpeningAngle", 12.0 * M_PI / 180.0, "0.5 * minimal opening angles of the used cameras"); 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", false, "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>("RandomNoiseLevel", 1.0f, "Maximum for the random noise that will be added to the localization necessities"); //defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used"); //defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used"); //defineOptionalProperty<std::string>("TCPControlUnitName", "TCPControlUnit", "Name of the tcp control unit component that should be used"); //defineOptionalProperty<std::string>("ObjectMemoryObserverName", "ObjectMemoryObserver", "Name of the ObjectMemoryObserver component that should be used"); //defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge component that should be used"); } }; /** * @defgroup Component-ViewSelection ViewSelection * @ingroup RobotComponents-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() */ virtual std::string getDefaultName() const { return "ViewSelection"; } protected: /** * @see armarx::ManagedIceObject::onInitComponent() */ virtual void onInitComponent(); /** * @see armarx::ManagedIceObject::onConnectComponent() */ virtual void onConnectComponent(); /** * @see armarx::ManagedIceObject::onDisconnectComponent() */ virtual void onDisconnectComponent(); /** * @see armarx::ManagedIceObject::onExitComponent() */ virtual void onExitComponent(); /** * @see PropertyUser::createPropertyDefinitions() */ virtual PropertyDefinitionsPtr createPropertyDefinitions(); virtual void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::Current()); virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) { ARMARX_INFO << "activating automatic view selection"; doAutomaticViewSelection = true; } virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) { ARMARX_INFO << "DEactivating automatic view selection"; doAutomaticViewSelection = false; } private: void process(); void addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc); void setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor = 1.0f); RobotStateComponentInterfacePrx robotStateComponent; memoryx::WorkingMemoryInterfacePrx memoryProxy; memoryx::PriorKnowledgeInterfacePrx priorKnowledge; memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesProxy; memoryx::ObjectClassMemorySegmentBasePrx objectClassesProxy; HeadIKUnitInterfacePrx headIKUnitProxy; DebugDrawerInterfacePrx drawer; std::string headIKKinematicChainName; std::string headFrameName, cameraFrameName; float randomNoiseLevel; float centralHeadTiltAngle; armarx::PeriodicTask<ViewSelection>::pointer_type processorTask; CIntensityGraph* saliencyEgosphereGraph; CIntensityGraph* visibilityMaskGraph; CIntensityGraph* randomNoiseGraph; CGraphPyramidLookupTable* graphLookupTable; std::vector<int> nodeVisitedForObject; float halfCameraOpeningAngle; float deviationFromCameraCenterFactor; float sleepingTimeBetweenViewDirectionChanges; IceUtil::Time timeOfLastViewChange; bool drawViewDirection; armarx::Mutex manualViewTargetsMutex; std::vector<FramedPositionBasePtr> manualViewTargets; bool doAutomaticViewSelection; std::map<std::string, memoryx::ObjectClassPtr> recognizableObjClasses; Eigen::Vector3f offsetToHeadCenter; }; } #endif