AffordancePipelineVisualization.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 2015
20  * @copyright http://www.gnu.org/licenses/gpl.txt
21  * GNU General Public License
22  */
23 
24 #pragma once
25 
28 
29 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
31 
32 #include <VisionX/interface/components/PointCloudSegmenter.h>
33 #include <VisionX/interface/components/AffordancePipelineVisualization.h>
35 
36 #include <AffordanceKit/primitives/Plane.h>
37 #include <AffordanceKit/primitives/Cylinder.h>
38 #include <AffordanceKit/primitives/Sphere.h>
39 #include <AffordanceKit/primitives/Box.h>
40 
41 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
45 
46 #include <Eigen/Eigen>
47 #include <IceUtil/IceUtil.h>
48 
49 #include <mutex>
50 
51 namespace armarx
52 {
53  /**
54  * @class AffordanceExtractionPropertyDefinitions
55  * @brief
56  */
59  {
60  public:
63  {
64  defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory component");
65  defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "The name of the debug drawer topic");
66  defineOptionalProperty<std::string>("DebugDrawerSelectionsTopicName", "DebugDrawerSelections", "The name of the debug drawer topic");
67  defineOptionalProperty<std::string>("VisualizationUpdateTopicName", "AffordanceVisualizationTopic", "Topic under which visualization updates are reported");
68  defineOptionalProperty<bool>("EnableDebugDrawerSelections", true, "Allow users to select primitives via debug drawer");
69 
70  defineOptionalProperty<bool>("EnablePrimitiveVisualization", true, "Initially enable primitive visualization");
71  defineOptionalProperty<bool>("EnableAffordanceVisualization", true, "Initially enable affordance visualization");
72  defineOptionalProperty<bool>("EnableDebugVisualization", false, "Initially enable debug visualization");
73 
74  defineOptionalProperty<std::string>("VisualizeAffordanceBeliefFunction", "", "The affordance belief function that should initially be visualized");
75  defineOptionalProperty<float>("MinExpectedProbability", 0, "Visualize only samplings with expected probability greater than X");
76 
77  defineOptionalProperty<bool>("NoAffordanceExtractionConfigured", false, "In this case visualization will start as soon as primitives are computed");
78 
79  defineOptionalProperty<std::string>("FileName", "", "Load affordances from a file name and exit.");
80  }
81  };
82 
83  /**
84  * @class AffordanceExtraction
85  * @ingroup RobotSkillTemplates-Components
86  * @brief A brief description
87  *
88  * Detailed Description
89  */
91  virtual public Component,
92  virtual public AffordancePipelineVisualizationInterface
93  {
94  public:
96 
97  /**
98  * @see armarx::ManagedIceObject::getDefaultName()
99  */
100  std::string getDefaultName() const override
101  {
102  return "AffordancePipelineVisualization";
103  }
104 
105  void reportNewPointCloudSegmentation(const Ice::Current& c = Ice::emptyCurrent) override;
106  void reportNewAffordances(const Ice::Current& c = Ice::emptyCurrent) override;
107 
108  void selectPrimitive(const std::string& id, const Ice::Current& c = Ice::emptyCurrent) override;
109  void deselectPrimitive(const std::string& id, const Ice::Current& c = Ice::emptyCurrent) override;
110  void deselectAllPrimitives(const Ice::Current& c = Ice::emptyCurrent) override;
111 
112  void enableVisualization(bool enablePrimitives, bool enableAffordances, bool enableDebugInformation, const Ice::Current& c = Ice::emptyCurrent) override;
113  void configureAffordanceVisualization(AffordanceVisualizationStyle style, const std::vector<std::string>& primitives, const std::vector<std::string>& affordances, float minExpectedProbability, const Ice::Current& c = Ice::emptyCurrent) override;
114  void triggerVisualization(const Ice::Current& c = Ice::emptyCurrent) override;
115 
116  void exportVisualizationAsIV(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
117  void exportVisualizationAsJSON(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
118 
119  void clearVisualization(const Ice::Current& c = Ice::emptyCurrent) override;
120 
121  // DebugDrawerListener interface
122  void reportSelectionChanged(const DebugDrawerSelectionList& selectedObjects, const ::Ice::Current& = Ice::emptyCurrent) override;
123 
124  protected:
125  /**
126  * @see armarx::ManagedIceObject::onInitComponent()
127  */
128  void onInitComponent() override;
129 
130  /**
131  * @see armarx::ManagedIceObject::onConnectComponent()
132  */
133  void onConnectComponent() override;
134 
135  /**
136  * @see armarx::ManagedIceObject::onDisconnectComponent()
137  */
138  void onDisconnectComponent() override;
139 
140  /**
141  * @see armarx::ManagedIceObject::onExitComponent()
142  */
143  void onExitComponent() override;
144 
145  /**
146  * @see PropertyUser::createPropertyDefinitions()
147  */
149 
150  void visualizeScene(const AffordanceKit::ScenePtr& scene, bool updatePrimitives = true, bool updateAffordances = true, bool updateDebugInfo = true);
151  void visualizePrimitives(const AffordanceKit::PrimitiveSetPtr& primitives);
152  void visualizePrimitive(const AffordanceKit::PrimitivePtr& primitive);
154  void visualizeAffordance(const AffordanceKit::AffordancePtr& affordance, const AffordanceKit::PrimitivePtr& primitve, unsigned int index);
155  void visualizeAffordanceLabels(const AffordanceKit::PrimitivePtr& primitive, const std::vector<std::string>& labels);
157 
158  void visualizePlane(const AffordanceKit::PlanePtr& plane);
159  void visualizeSphere(const AffordanceKit::SpherePtr& sphere);
160  void visualizeCylinder(const AffordanceKit::CylinderPtr& cylinder);
161  void visualizeBox(const AffordanceKit::BoxPtr& box);
162 
163  protected:
164  void visualize(bool updatePrimitives = true, bool updateAffordances = true, bool updateDebugInfo = true);
165 
166  protected:
168  AffordancePipelineVisualizationListenerPrx affordanceVisualizationTopicPrx;
169  memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
170  memoryx::AffordanceSegmentBasePrx affordanceSegment;
171  memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment;
172 
173  std::set<std::string> selections;
174 
175  std::map<int, std::list<std::string>> labelMap;
176 
177  DrawColor primitiveColor;
180 
182 
186 
187  AffordanceVisualizationStyle visualizationStyle;
188  std::vector<std::string> visualizationPrimitiveSet;
189  std::vector<std::string> visualizationAffordanceSet;
191 
192  std::mutex sceneMutex;
194  };
195 }
armarx::AffordancePipelineVisualization::reportSelectionChanged
void reportSelectionChanged(const DebugDrawerSelectionList &selectedObjects, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:614
armarx::AffordancePipelineVisualization::visualizePrimitive
void visualizePrimitive(const AffordanceKit::PrimitivePtr &primitive)
Definition: AffordancePipelineVisualization.cpp:282
armarx::AffordancePipelineVisualization::visualizationStyle
AffordanceVisualizationStyle visualizationStyle
Definition: AffordancePipelineVisualization.h:187
armarx::AffordancePipelineVisualization::configureAffordanceVisualization
void configureAffordanceVisualization(AffordanceVisualizationStyle style, const std::vector< std::string > &primitives, const std::vector< std::string > &affordances, float minExpectedProbability, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:241
armarx::AffordancePipelineVisualization::primitiveColorFrame
DrawColor primitiveColorFrame
Definition: AffordancePipelineVisualization.h:178
SegmentedMemory.h
armarx::AffordancePipelineVisualization::selections
std::set< std::string > selections
Definition: AffordancePipelineVisualization.h:173
armarx::AffordancePipelineVisualization::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: AffordancePipelineVisualization.cpp:161
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::AffordancePipelineVisualization::selectPrimitive
void selectPrimitive(const std::string &id, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:194
armarx::AffordancePipelineVisualization::visualizeAffordance
void visualizeAffordance(const AffordanceKit::AffordancePtr &affordance, const AffordanceKit::PrimitivePtr &primitve, unsigned int index)
Definition: AffordancePipelineVisualization.cpp:348
armarx::AffordancePipelineVisualization::deselectPrimitive
void deselectPrimitive(const std::string &id, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:204
armarx::AffordancePipelineVisualization::environmentalPrimitiveSegment
memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment
Definition: AffordancePipelineVisualization.h:171
armarx::AffordancePipelineVisualization::onDisconnectComponent
void onDisconnectComponent() override
Definition: AffordancePipelineVisualization.cpp:153
armarx::AffordancePipelineVisualization::affordanceVisualizationTopicPrx
AffordancePipelineVisualizationListenerPrx affordanceVisualizationTopicPrx
Definition: AffordancePipelineVisualization.h:168
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::AffordancePipelineVisualization::deselectAllPrimitives
void deselectAllPrimitives(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:214
memoryx::AffordancePtr
IceInternal::Handle< Affordance > AffordancePtr
Definition: Affordance.h:40
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::AffordancePipelineVisualization::enableVisualization
void enableVisualization(bool enablePrimitives, bool enableAffordances, bool enableDebugInformation, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:224
armarx::AffordancePipelineVisualization::debugVisualizationEnabled
bool debugVisualizationEnabled
Definition: AffordancePipelineVisualization.h:185
EnvironmentalPrimitiveSegment.h
armarx::AffordancePipelineVisualization::affordanceSegment
memoryx::AffordanceSegmentBasePrx affordanceSegment
Definition: AffordancePipelineVisualization.h:170
RunningTask.h
scene3D::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: PointerDefinitions.h:36
armarx::AffordancePipelineVisualization::visualizationAffordanceSet
std::vector< std::string > visualizationAffordanceSet
Definition: AffordancePipelineVisualization.h:189
armarx::AffordancePipelineVisualization::labelMap
std::map< int, std::list< std::string > > labelMap
Definition: AffordancePipelineVisualization.h:175
armarx::AffordancePipelineVisualization::scene
AffordanceKitArmarX::SceneArmarXPtr scene
Definition: AffordancePipelineVisualization.h:193
armarx::AffordancePipelineVisualization::primitiveColor
DrawColor primitiveColor
Definition: AffordancePipelineVisualization.h:177
armarx::AffordancePipelineVisualization::exportVisualizationAsJSON
void exportVisualizationAsJSON(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:549
armarx::AffordancePipelineVisualization
Definition: AffordancePipelineVisualization.h:90
armarx::AffordancePipelineVisualization::debugDrawerTopicPrx
DebugDrawerInterfacePrx debugDrawerTopicPrx
Definition: AffordancePipelineVisualization.h:167
armarx::AffordancePipelineVisualization::onExitComponent
void onExitComponent() override
Definition: AffordancePipelineVisualization.cpp:157
armarx::AffordancePipelineVisualization::visualizeDebugInformation
void visualizeDebugInformation(const AffordanceKit::ScenePtr &scene)
Definition: AffordancePipelineVisualization.cpp:395
FramedPose.h
armarx::AffordancePipelineVisualization::sceneMutex
std::mutex sceneMutex
Definition: AffordancePipelineVisualization.h:192
armarx::AffordancePipelineVisualization::exportVisualizationAsIV
void exportVisualizationAsIV(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:542
armarx::AffordancePipelineVisualization::reportNewAffordances
void reportNewAffordances(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:180
armarx::AffordancePipelineVisualization::visualizeSphere
void visualizeSphere(const AffordanceKit::SpherePtr &sphere)
Definition: AffordancePipelineVisualization.cpp:412
armarx::AffordancePipelineVisualization::visualizeAffordanceLabels
void visualizeAffordanceLabels(const AffordanceKit::PrimitivePtr &primitive, const std::vector< std::string > &labels)
Definition: AffordancePipelineVisualization.cpp:382
armarx::AffordancePipelineVisualization::workingMemoryPrx
memoryx::WorkingMemoryInterfacePrx workingMemoryPrx
Definition: AffordancePipelineVisualization.h:169
filename
std::string filename
Definition: VisualizationRobot.cpp:83
AffordanceSegment.h
armarx::AffordancePipelineVisualization::affordanceLabelSize
int affordanceLabelSize
Definition: AffordancePipelineVisualization.h:181
armarx::AffordancePipelineVisualization::AffordancePipelineVisualization
AffordancePipelineVisualization()
Definition: AffordancePipelineVisualization.cpp:61
armarx::AffordancePipelineVisualization::reportNewPointCloudSegmentation
void reportNewPointCloudSegmentation(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:166
SceneArmarX.h
armarx::AffordancePipelineVisualizationPropertyDefinitions::AffordancePipelineVisualizationPropertyDefinitions
AffordancePipelineVisualizationPropertyDefinitions(std::string prefix)
Definition: AffordancePipelineVisualization.h:61
armarx::AffordancePipelineVisualization::visualizeScene
void visualizeScene(const AffordanceKit::ScenePtr &scene, bool updatePrimitives=true, bool updateAffordances=true, bool updateDebugInfo=true)
Definition: AffordancePipelineVisualization.cpp:256
armarx::AffordancePipelineVisualization::visualizePrimitives
void visualizePrimitives(const AffordanceKit::PrimitiveSetPtr &primitives)
Definition: AffordancePipelineVisualization.cpp:274
armarx::AffordancePipelineVisualization::getDefaultName
std::string getDefaultName() const override
Definition: AffordancePipelineVisualization.h:100
armarx::AffordancePipelineVisualizationPropertyDefinitions
Definition: AffordancePipelineVisualization.h:57
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
armarx::AffordancePipelineVisualization::clearVisualization
void clearVisualization(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:594
armarx::AffordancePipelineVisualization::visualizePlane
void visualizePlane(const AffordanceKit::PlanePtr &plane)
Definition: AffordancePipelineVisualization.cpp:399
armarx::AffordancePipelineVisualization::visualizeCylinder
void visualizeCylinder(const AffordanceKit::CylinderPtr &cylinder)
Definition: AffordancePipelineVisualization.cpp:418
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
IceUtil::Handle
Definition: forward_declarations.h:29
armarx::AffordancePipelineVisualization::triggerVisualization
void triggerVisualization(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:251
armarx::AffordancePipelineVisualization::primitiveVisualizationEnabled
bool primitiveVisualizationEnabled
Definition: AffordancePipelineVisualization.h:183
armarx::AffordancePipelineVisualization::visualizeAffordances
void visualizeAffordances(const AffordanceKit::ScenePtr &scene)
Definition: AffordancePipelineVisualization.cpp:307
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
AffordanceKitArmarX::SceneArmarXPtr
std::shared_ptr< SceneArmarX > SceneArmarXPtr
Definition: SceneArmarX.h:49
armarx::AffordancePipelineVisualization::onInitComponent
void onInitComponent() override
Definition: AffordancePipelineVisualization.cpp:70
armarx::AffordancePipelineVisualization::affordanceLabelColor
DrawColor affordanceLabelColor
Definition: AffordancePipelineVisualization.h:179
armarx::AffordancePipelineVisualization::visualizationMinExpectedProbability
float visualizationMinExpectedProbability
Definition: AffordancePipelineVisualization.h:190
armarx::AffordancePipelineVisualization::onConnectComponent
void onConnectComponent() override
Definition: AffordancePipelineVisualization.cpp:99
armarx::AffordancePipelineVisualization::visualize
void visualize(bool updatePrimitives=true, bool updateAffordances=true, bool updateDebugInfo=true)
Definition: AffordancePipelineVisualization.cpp:477
armarx::AffordancePipelineVisualization::visualizationPrimitiveSet
std::vector< std::string > visualizationPrimitiveSet
Definition: AffordancePipelineVisualization.h:188
armarx::AffordancePipelineVisualization::visualizeBox
void visualizeBox(const AffordanceKit::BoxPtr &box)
Definition: AffordancePipelineVisualization.cpp:428
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::AffordancePipelineVisualization::affordanceVisualizationEnabled
bool affordanceVisualizationEnabled
Definition: AffordancePipelineVisualization.h:184