AffordanceExtraction.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as
6  * published by the Free Software Foundation; either version 2 of
7  * the License, or (at your option) any later version.
8  *
9  * ArmarX is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  *
17  * @package VisionX
18  * @author Peter Kaiser (peter dot kaiser at kit dot edu)
19  * @date 2014
20  * @copyright http://www.gnu.org/licenses/gpl.txt
21  * GNU General Public License
22  */
23 
24 #pragma once
25 
26 
27 #include <ArmarXCore/interface/observers/ObserverInterface.h>
28 
31 
32 
33 #include <VisionX/interface/components/PointCloudSegmenter.h>
34 #include <VisionX/interface/components/AffordanceExtraction.h>
35 
36 #include <AffordanceKit/affordances/PlatformGraspAffordance.h>
37 #include <AffordanceKit/affordances/PrismaticGraspAffordance.h>
38 #include <AffordanceKit/affordances/GraspAffordance.h>
39 #include <AffordanceKit/affordances/SupportAffordance.h>
40 #include <AffordanceKit/affordances/LeanAffordance.h>
41 #include <AffordanceKit/affordances/HoldAffordance.h>
42 #include <AffordanceKit/affordances/LiftAffordance.h>
43 #include <AffordanceKit/affordances/TurnAffordance.h>
44 #include <AffordanceKit/affordances/PushAffordance.h>
45 #include <AffordanceKit/affordances/PullAffordance.h>
46 #include <AffordanceKit/affordances/BimanualPlatformGraspAffordance.h>
47 #include <AffordanceKit/affordances/BimanualPrismaticGraspAffordance.h>
48 #include <AffordanceKit/affordances/BimanualGraspAffordance.h>
49 #include <AffordanceKit/affordances/BimanualOpposedGraspAffordance.h>
50 #include <AffordanceKit/affordances/BimanualAlignedGraspAffordance.h>
51 #include <AffordanceKit/affordances/BimanualOpposedPlatformGraspAffordance.h>
52 #include <AffordanceKit/affordances/BimanualOpposedPrismaticGraspAffordance.h>
53 #include <AffordanceKit/affordances/BimanualAlignedPlatformGraspAffordance.h>
54 #include <AffordanceKit/affordances/BimanualAlignedPrismaticGraspAffordance.h>
55 #include <AffordanceKit/affordances/BimanualTurnAffordance.h>
56 #include <AffordanceKit/affordances/BimanualLiftAffordance.h>
57 
58 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
62 
63 #include <Eigen/Eigen>
64 #include <mutex>
65 
66 namespace armarx
67 {
68  /**
69  * @class AffordanceExtractionPropertyDefinitions
70  * @brief
71  */
74  {
75  public:
78  {
79  defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
80  defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory component");
81  defineOptionalProperty<std::string>("AffordanceExtractionTopicName", "ExtractedAffordances", "Topic for publishing extraction notifications");
82  defineOptionalProperty<std::string>("PrimitiveExtractionTopicName", "SegmentedPointCloud", "Topic under which primitive extraction notifications are published");
83  defineOptionalProperty<std::string>("EmbodimentName", "WalkMan", "Name of the embodiment to use for affordance extraction (Armar3, Armar4, WalkMan");
84  defineOptionalProperty<float>("SpatialSamplingDistance", 75, "Distance between two spatial samplings in mm");
85  defineOptionalProperty<int>("NumOrientationalSamplings", 4, "Number of orientational samplings");
86  defineOptionalProperty<std::string>("RestrictToAffordanceList", "", "Only evaluate affordances from this list (use short names in comma separated list)");
87  defineOptionalProperty<int>("MaxPrimitiveSamplingSize", 0, "Maximum size of primitive sampling to be considered for extracting affordances (0 means infinity)");
88  defineOptionalProperty<float>("AffordanceExtractionCertainty", 0.8, "Certainty value attributed to visual affordance extraction");
89  }
90  };
91 
92  /**
93  * @class AffordanceExtraction
94  * @ingroup RobotSkillTemplates-Components
95  * @brief A brief description
96  *
97  * Detailed Description
98  */
100  virtual public Component,
101  virtual public AffordanceExtractionInterface
102  {
103  public:
105 
106  /**
107  * @see armarx::ManagedIceObject::getDefaultName()
108  */
109  std::string getDefaultName() const override
110  {
111  return "AffordanceExtraction";
112  }
113 
114  void reportNewPointCloudSegmentation(const Ice::Current& c = Ice::emptyCurrent) override;
115 
116  void enablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
117  void disablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
118  bool isPipelineStepEnabled(const Ice::Current& c = Ice::emptyCurrent) override;
119  armarx::TimestampBasePtr getLastProcessedTimestamp(const Ice::Current& c = Ice::emptyCurrent) override;
120 
121  void addObservation(const PoseBasePtr& endEffectorPose, const std::string& affordance, const std::string& primitiveId, float result, float certainty, float sigma_pos, float sigma_rot, const Ice::Current& c = Ice::emptyCurrent) override;
122 
123  void exportScene(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
124  void importScene(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
125 
126  protected:
127  /**
128  * @see armarx::ManagedIceObject::onInitComponent()
129  */
130  void onInitComponent() override;
131 
132  /**
133  * @see armarx::ManagedIceObject::onConnectComponent()
134  */
135  void onConnectComponent() override;
136 
137  /**
138  * @see armarx::ManagedIceObject::onDisconnectComponent()
139  */
140  void onDisconnectComponent() override;
141 
142  /**
143  * @see armarx::ManagedIceObject::onExitComponent()
144  */
145  void onExitComponent() override;
146 
147  /**
148  * @see PropertyUser::createPropertyDefinitions()
149  */
151 
153 
154  protected:
155  AffordanceExtractionListenerPrx listenerPrx;
156 
157  std::mutex affordancesMutex;
158 
159  std::mutex primitivesMutex;
161 
162  std::mutex timestampMutex;
164 
165  std::mutex enableMutex;
166  bool enabled;
167 
169 
170  memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
171  memoryx::AffordanceSegmentBasePrx affordanceSegment;
172  memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment;
173 
174  AffordanceKit::PrimitiveSetPtr primitiveSegmentation;
175 
176  AffordanceKit::EmbodimentPtr embodiment;
177 
178  AffordanceKit::PlatformGraspAffordancePtr platformGraspAffordance;
179  AffordanceKit::PrismaticGraspAffordancePtr prismaticGraspAffordance;
180  AffordanceKit::GraspAffordancePtr graspAffordance;
181 
182  AffordanceKit::SupportAffordancePtr supportAffordance;
183  AffordanceKit::LeanAffordancePtr leanAffordance;
184  AffordanceKit::HoldAffordancePtr holdAffordance;
185  AffordanceKit::LiftAffordancePtr liftAffordance;
186  AffordanceKit::TurnAffordancePtr turnAffordance;
187  AffordanceKit::PushAffordancePtr pushAffordance;
188  AffordanceKit::PullAffordancePtr pullAffordance;
189 
190  AffordanceKit::BimanualPlatformGraspAffordancePtr bimanualPlatformGraspAffordance;
191  AffordanceKit::BimanualPrismaticGraspAffordancePtr bimanualPrismaticGraspAffordance;
192  AffordanceKit::BimanualGraspAffordancePtr bimanualGraspAffordance;
193  AffordanceKit::BimanualOpposedGraspAffordancePtr bimanualOpposedGraspAffordance;
194  AffordanceKit::BimanualAlignedGraspAffordancePtr bimanualAlignedGraspAffordance;
195  AffordanceKit::BimanualOpposedPlatformGraspAffordancePtr bimanualOpposedPlatformGraspAffordance;
196  AffordanceKit::BimanualOpposedPrismaticGraspAffordancePtr bimanualOpposedPrismaticGraspAffordance;
197  AffordanceKit::BimanualAlignedPlatformGraspAffordancePtr bimanualAlignedPlatformGraspAffordance;
198  AffordanceKit::BimanualAlignedPrismaticGraspAffordancePtr bimanualAlignedPrismaticGraspAffordance;
199  AffordanceKit::BimanualTurnAffordancePtr bimanualTurnAffordance;
200  AffordanceKit::BimanualLiftAffordancePtr bimanualLiftAffordance;
201 
202  std::map<std::string, memoryx::AffordanceType> affordanceTypes;
203 
204  std::vector<AffordanceKit::UnimanualAffordancePtr> unimanualAffordances;
205  std::vector<AffordanceKit::BimanualAffordancePtr> bimanualAffordances;
206 
207  std::set<std::string> affordanceRestriction;
208 
211 
212 
214  };
215 }
216 
armarx::AffordanceExtraction::affordancesMutex
std::mutex affordancesMutex
Definition: AffordanceExtraction.h:157
armarx::AffordanceExtraction::pushAffordance
AffordanceKit::PushAffordancePtr pushAffordance
Definition: AffordanceExtraction.h:187
armarx::AffordanceExtraction::onExitComponent
void onExitComponent() override
Definition: AffordanceExtraction.cpp:239
armarx::AffordanceExtraction::getLastProcessedTimestamp
armarx::TimestampBasePtr getLastProcessedTimestamp(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:488
armarx::AffordanceExtraction::onInitComponent
void onInitComponent() override
Definition: AffordanceExtraction.cpp:66
SegmentedMemory.h
armarx::AffordanceExtraction::lastProcessedTimestamp
long lastProcessedTimestamp
Definition: AffordanceExtraction.h:163
armarx::AffordanceExtraction::affordanceRestriction
std::set< std::string > affordanceRestriction
Definition: AffordanceExtraction.h:207
armarx::AffordanceExtraction::bimanualOpposedPrismaticGraspAffordance
AffordanceKit::BimanualOpposedPrismaticGraspAffordancePtr bimanualOpposedPrismaticGraspAffordance
Definition: AffordanceExtraction.h:196
armarx::AffordanceExtraction::liftAffordance
AffordanceKit::LiftAffordancePtr liftAffordance
Definition: AffordanceExtraction.h:185
armarx::AffordanceExtraction::timestampMutex
std::mutex timestampMutex
Definition: AffordanceExtraction.h:162
armarx::AffordanceExtraction::environmentalPrimitiveSegment
memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment
Definition: AffordanceExtraction.h:172
armarx::AffordanceExtraction::bimanualPlatformGraspAffordance
AffordanceKit::BimanualPlatformGraspAffordancePtr bimanualPlatformGraspAffordance
Definition: AffordanceExtraction.h:190
armarx::AffordanceExtraction::affordanceExtractionThread
void affordanceExtractionThread()
Definition: AffordanceExtraction.cpp:311
armarx::AffordanceExtraction::getDefaultName
std::string getDefaultName() const override
Definition: AffordanceExtraction.h:109
armarx::AffordanceExtraction::bimanualOpposedPlatformGraspAffordance
AffordanceKit::BimanualOpposedPlatformGraspAffordancePtr bimanualOpposedPlatformGraspAffordance
Definition: AffordanceExtraction.h:195
armarx::AffordanceExtraction::bimanualLiftAffordance
AffordanceKit::BimanualLiftAffordancePtr bimanualLiftAffordance
Definition: AffordanceExtraction.h:200
armarx::AffordanceExtraction::embodiment
AffordanceKit::EmbodimentPtr embodiment
Definition: AffordanceExtraction.h:176
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::AffordanceExtraction::importScene
void importScene(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:300
armarx::AffordanceExtraction::enabled
bool enabled
Definition: AffordanceExtraction.h:166
PeriodicTask.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
EnvironmentalPrimitiveSegment.h
armarx::AffordanceExtraction::enableMutex
std::mutex enableMutex
Definition: AffordanceExtraction.h:165
armarx::AffordanceExtraction::isPipelineStepEnabled
bool isPipelineStepEnabled(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:482
armarx::AffordanceExtraction::prismaticGraspAffordance
AffordanceKit::PrismaticGraspAffordancePtr prismaticGraspAffordance
Definition: AffordanceExtraction.h:179
armarx::AffordanceExtraction::workingMemoryPrx
memoryx::WorkingMemoryInterfacePrx workingMemoryPrx
Definition: AffordanceExtraction.h:170
armarx::AffordanceExtraction::leanAffordance
AffordanceKit::LeanAffordancePtr leanAffordance
Definition: AffordanceExtraction.h:183
armarx::AffordanceExtraction::enablePipelineStep
void enablePipelineStep(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:466
armarx::AffordanceExtraction::unimanualAffordances
std::vector< AffordanceKit::UnimanualAffordancePtr > unimanualAffordances
Definition: AffordanceExtraction.h:204
armarx::AffordanceExtractionPropertyDefinitions
Definition: AffordanceExtraction.h:72
armarx::AffordanceExtraction::turnAffordance
AffordanceKit::TurnAffordancePtr turnAffordance
Definition: AffordanceExtraction.h:186
armarx::AffordanceExtraction::onDisconnectComponent
void onDisconnectComponent() override
Definition: AffordanceExtraction.cpp:234
armarx::AffordanceExtraction::bimanualAffordances
std::vector< AffordanceKit::BimanualAffordancePtr > bimanualAffordances
Definition: AffordanceExtraction.h:205
armarx::AffordanceExtraction::addObservation
void addObservation(const PoseBasePtr &endEffectorPose, const std::string &affordance, const std::string &primitiveId, float result, float certainty, float sigma_pos, float sigma_rot, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:243
armarx::AffordanceExtraction::pullAffordance
AffordanceKit::PullAffordancePtr pullAffordance
Definition: AffordanceExtraction.h:188
armarx::AffordanceExtraction::supportAffordance
AffordanceKit::SupportAffordancePtr supportAffordance
Definition: AffordanceExtraction.h:182
armarx::AffordanceExtractionPropertyDefinitions::AffordanceExtractionPropertyDefinitions
AffordanceExtractionPropertyDefinitions(std::string prefix)
Definition: AffordanceExtraction.h:76
filename
std::string filename
Definition: VisualizationRobot.cpp:83
AffordanceSegment.h
armarx::AffordanceExtraction::affordanceTypes
std::map< std::string, memoryx::AffordanceType > affordanceTypes
Definition: AffordanceExtraction.h:202
armarx::AffordanceExtraction::primitiveSegmentation
AffordanceKit::PrimitiveSetPtr primitiveSegmentation
Definition: AffordanceExtraction.h:174
armarx::AffordanceExtraction::disablePipelineStep
void disablePipelineStep(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:474
armarx::AffordanceExtraction::bimanualGraspAffordance
AffordanceKit::BimanualGraspAffordancePtr bimanualGraspAffordance
Definition: AffordanceExtraction.h:192
armarx::AffordanceExtraction::graspAffordance
AffordanceKit::GraspAffordancePtr graspAffordance
Definition: AffordanceExtraction.h:180
armarx::AffordanceExtraction::bimanualAlignedPrismaticGraspAffordance
AffordanceKit::BimanualAlignedPrismaticGraspAffordancePtr bimanualAlignedPrismaticGraspAffordance
Definition: AffordanceExtraction.h:198
armarx::AffordanceExtraction::bimanualOpposedGraspAffordance
AffordanceKit::BimanualOpposedGraspAffordancePtr bimanualOpposedGraspAffordance
Definition: AffordanceExtraction.h:193
armarx::AffordanceExtraction::AffordanceExtraction
AffordanceExtraction()
Definition: AffordanceExtraction.cpp:42
armarx::AffordanceExtraction::primitivesMutex
std::mutex primitivesMutex
Definition: AffordanceExtraction.h:159
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
armarx::AffordanceExtraction::bimanualPrismaticGraspAffordance
AffordanceKit::BimanualPrismaticGraspAffordancePtr bimanualPrismaticGraspAffordance
Definition: AffordanceExtraction.h:191
armarx::AffordanceExtraction::hasNewPrimitives
bool hasNewPrimitives
Definition: AffordanceExtraction.h:160
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::AffordanceExtraction::bimanualAlignedPlatformGraspAffordance
AffordanceKit::BimanualAlignedPlatformGraspAffordancePtr bimanualAlignedPlatformGraspAffordance
Definition: AffordanceExtraction.h:197
armarx::AffordanceExtraction::affordanceExtractionTask
PeriodicTask< AffordanceExtraction >::pointer_type affordanceExtractionTask
Definition: AffordanceExtraction.h:168
IceUtil::Handle
Definition: forward_declarations.h:29
armarx::AffordanceExtraction::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: AffordanceExtraction.cpp:461
armarx::AffordanceExtraction::onConnectComponent
void onConnectComponent() override
Definition: AffordanceExtraction.cpp:201
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
armarx::AffordanceExtraction
A brief description.
Definition: AffordanceExtraction.h:99
armarx::AffordanceExtraction::platformGraspAffordance
AffordanceKit::PlatformGraspAffordancePtr platformGraspAffordance
Definition: AffordanceExtraction.h:178
armarx::AffordanceExtraction::reportNewPointCloudSegmentation
void reportNewPointCloudSegmentation(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:49
armarx::AffordanceExtraction::bimanualAlignedGraspAffordance
AffordanceKit::BimanualAlignedGraspAffordancePtr bimanualAlignedGraspAffordance
Definition: AffordanceExtraction.h:194
armarx::AffordanceExtraction::holdAffordance
AffordanceKit::HoldAffordancePtr holdAffordance
Definition: AffordanceExtraction.h:184
armarx::AffordanceExtraction::exportScene
void exportScene(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordanceExtraction.cpp:292
armarx::AffordanceExtraction::listenerPrx
AffordanceExtractionListenerPrx listenerPrx
Definition: AffordanceExtraction.h:155
armarx::AffordanceExtraction::bimanualTurnAffordance
AffordanceKit::BimanualTurnAffordancePtr bimanualTurnAffordance
Definition: AffordanceExtraction.h:199
armarx::AffordanceExtraction::affordanceExtractionCertainty
float affordanceExtractionCertainty
Definition: AffordanceExtraction.h:210
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::AffordanceExtraction::affordanceSegment
memoryx::AffordanceSegmentBasePrx affordanceSegment
Definition: AffordanceExtraction.h:171
armarx::AffordanceExtraction::debugObserver
DebugObserverInterfacePrx debugObserver
Definition: AffordanceExtraction.h:213
armarx::AffordanceExtraction::maxPrimitiveSamplingSize
unsigned int maxPrimitiveSamplingSize
Definition: AffordanceExtraction.h:209