Newer
Older
/*
* 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
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
#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", 50.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)");
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");
}
};
/**
* @ingroup RobotComponents-Components
* @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
* of the current requested robot 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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
*/
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())
{
doAutomaticViewSelection = true;
}
virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
{
doAutomaticViewSelection = false;
}
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
private:
void process();
void addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex);
void setRandomNoise();
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;
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