/*
* 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