AffordancePipelineVisualization.cpp
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 
25 
27 
32 
35 
36 #include <VirtualRobot/MathTools.h>
37 #include <SimoxUtility/algorithm/string/string_tools.h>
38 
39 #include <pcl/point_types.h>
40 #include <pcl/common/colors.h>
41 
42 #include <memory>
43 
44 #include <fstream>
45 #include <cmath>
46 
47 // Alternate layer names in order to have safe clears,
48 // even in case of overtaking TCP calls
49 #define PRIMITIVE_LAYER_NAME(n) (std::string("primitivesLayer") + std::string(((n)%2 == 0)? "" : "_2"))
50 #define PRIMITIVE_LABELS_LAYER_NAME(n) (std::string("primitiveLabelsLayer") + std::string(((n)%2 == 0)? "" : "_2"))
51 #define PRIMITIVE_FRAMES_LAYER_NAME(n) (std::string("primitiveFramesLayer") + std::string(((n)%2 == 0)? "" : "_2"))
52 #define PRIMITIVE_GRASP_POINTS_LAYER_NAME(n) (std::string("graspPointLayer") + std::string(((n)%2 == 0)? "" : "_2"))
53 #define AFFORDANCES_LAYER_NAME(n) (std::string("affordancesLayer") + std::string(((n)%2 == 0)? "" : "_2"))
54 
58 
59 using namespace armarx;
60 
62  affordanceLabelSize(18),
63  primitiveVisualizationEnabled(true),
64  affordanceVisualizationEnabled(true),
65  debugVisualizationEnabled(false),
66  visualizationStyle(AffordanceVisualizationStyle::eAffordanceLabels)
67 {
68 }
69 
71 {
72  offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
73  offeringTopic(getProperty<std::string>("VisualizationUpdateTopicName").getValue());
74  usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
75  usingTopic("ExtractedAffordances");
76  usingTopic("SegmentedPointCloud");
77 
78  primitiveColor = {0.4, 0.6, 1.0, 0.7};
79  primitiveColorFrame = {0.2, 0.4, 0.8, 0.7};
80  affordanceLabelColor = {0.96, 0.82, 0.3, 1};
81 
82  primitiveVisualizationEnabled = getProperty<bool>("EnablePrimitiveVisualization").getValue();
83  affordanceVisualizationEnabled = getProperty<bool>("EnableAffordanceVisualization").getValue();
84  debugVisualizationEnabled = getProperty<bool>("EnableDebugVisualization").getValue();
85 
86  std::string beliefFunction = getProperty<std::string>("VisualizeAffordanceBeliefFunction").getValue();
87 
88  if (beliefFunction != "")
89  {
90  std::vector<std::string> primitives;
91 
92  std::vector<std::string> affordances;
93  affordances.push_back(beliefFunction);
94 
95  configureAffordanceVisualization(eAffordanceBeliefFunctions, primitives, affordances, getProperty<float>("MinExpectedProbability").getValue());
96  }
97 }
98 
100 {
101 
102 
103  debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
104  if (!debugDrawerTopicPrx)
105  {
106  ARMARX_ERROR << "Failed to obtain debug drawer proxy";
107  return;
108  }
109 
110  affordanceVisualizationTopicPrx = getTopic<AffordancePipelineVisualizationListenerPrx>(getProperty<std::string>("VisualizationUpdateTopicName").getValue());
112  {
113  ARMARX_ERROR << "Failed to obtain affordance visualization topic proxy";
114  return;
115  }
116 
117  usingTopic(getProperty<std::string>("DebugDrawerSelectionsTopicName").getValue());
118  offeringTopic(getProperty<std::string>("VisualizationUpdateTopicName").getValue());
119 
120  std::string fileName = getProperty<std::string>("FileName").getValue();
121  if(!fileName.empty())
122  {
123  ARMARX_INFO << "Visualizing affordances from file " << VAROUT(fileName);
125  scene->load(fileName);
126  visualize();
127  ARMARX_INFO << "Done visualizing affordances.";
128  return;
129  }
130 
131  workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
132  if (!workingMemoryPrx)
133  {
134  ARMARX_ERROR << "Failed to obtain working memory proxy";
135  return;
136  }
137 
138  affordanceSegment = workingMemoryPrx->getAffordanceSegment();
139  if (!affordanceSegment)
140  {
141  ARMARX_ERROR << "Failed to obtain affordance segment pointer";
142  return;
143  }
144 
145  environmentalPrimitiveSegment = workingMemoryPrx->getEnvironmentalPrimitiveSegment();
147  {
148  ARMARX_ERROR << "Failed to obtain environmental primitive segment pointer";
149  return;
150  }
151 }
152 
154 {
155 }
156 
158 {
159 }
160 
162 {
164 }
165 
167 {
168  ARMARX_INFO << "Receiving new primitives";
169 
170  if (getProperty<bool>("NoAffordanceExtractionConfigured").getValue())
171  {
172  sceneMutex.lock();
174  sceneMutex.unlock();
175 
176  visualize();
177  }
178 }
179 
181 {
182  ARMARX_INFO << "Receiving new affordances";
183 
184  if (!getProperty<bool>("NoAffordanceExtractionConfigured").getValue())
185  {
186  sceneMutex.lock();
188  sceneMutex.unlock();
189 
190  visualize();
191  }
192 }
193 
194 void AffordancePipelineVisualization::selectPrimitive(const std::string& id, const Ice::Current& c)
195 {
196  selections.insert(id);
197 
198  if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
199  {
201  }
202 }
203 
204 void AffordancePipelineVisualization::deselectPrimitive(const std::string& id, const Ice::Current& c)
205 {
206  selections.erase(id);
207 
208  if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
209  {
211  }
212 }
213 
215 {
216  selections.clear();
217 
218  if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
219  {
221  }
222 }
223 
224 void AffordancePipelineVisualization::enableVisualization(bool enablePrimitives, bool enableAffordances, bool enableDebugInformation, const Ice::Current& c)
225 {
226  ARMARX_INFO << "Visualization configuration changed: primitives=" << (enablePrimitives ? "enabled" : "disabled")
227  << ", affordances=" << (enableAffordances ? "enabled" : "disabled")
228  << ", debug=" << (enableDebugInformation ? "enabled" : "disabled");
229 
230  bool updatePrimitives = (primitiveVisualizationEnabled != enablePrimitives);
231  bool updateAffordances = (affordanceVisualizationEnabled != enableAffordances);
232  bool updateDebugInfo = (debugVisualizationEnabled != enableDebugInformation);
233 
234  primitiveVisualizationEnabled = enablePrimitives;
235  affordanceVisualizationEnabled = enableAffordances;
236  debugVisualizationEnabled = enableDebugInformation;
237 
238  visualize(updatePrimitives, updateAffordances, updateDebugInfo);
239 }
240 
241 void AffordancePipelineVisualization::configureAffordanceVisualization(AffordanceVisualizationStyle style, const std::vector<std::string>& primitives, const std::vector<std::string>& affordances, float minExpectedProbability, const Ice::Current& c)
242 {
243  visualizationStyle = style;
244  visualizationPrimitiveSet = primitives;
245  visualizationAffordanceSet = affordances;
246  visualizationMinExpectedProbability = minExpectedProbability;
247 
248  visualize(true, true, false);
249 }
250 
252 {
253  visualize();
254 }
255 
256 void AffordancePipelineVisualization::visualizeScene(const AffordanceKit::ScenePtr& scene, bool updatePrimitives, bool updateAffordances, bool updateDebugInfo)
257 {
258  if (primitiveVisualizationEnabled && updatePrimitives)
259  {
260  visualizePrimitives(scene->getPrimitives());
261  }
262 
263  if (affordanceVisualizationEnabled && updateAffordances)
264  {
266  }
267 
268  if (debugVisualizationEnabled && updateDebugInfo)
269  {
271  }
272 }
273 
274 void AffordancePipelineVisualization::visualizePrimitives(const AffordanceKit::PrimitiveSetPtr& primitives)
275 {
276  for (auto& primitive : *primitives)
277  {
278  visualizePrimitive(primitive);
279  }
280 }
281 
282 void AffordancePipelineVisualization::visualizePrimitive(const AffordanceKit::PrimitivePtr& primitive)
283 {
284  if (auto plane = std::dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
285  {
286  visualizePlane(plane);
287  }
288  else if (auto cyl = std::dynamic_pointer_cast<AffordanceKit::Cylinder>(primitive))
289  {
290  visualizeCylinder(cyl);
291  }
292  else if (auto sphere = std::dynamic_pointer_cast<AffordanceKit::Sphere>(primitive))
293  {
294  visualizeSphere(sphere);
295  }
296  else if (auto box = std::dynamic_pointer_cast<AffordanceKit::Box>(primitive))
297  {
298  visualizeBox(box);
299  }
300  else
301  {
302  ARMARX_WARNING << "Attempt to visualize primitive of unknown type";
303  return;
304  }
305 }
306 
308 {
309  AffordanceKit::PrimitiveSetPtr primitives = scene->getPrimitives();
310  const std::vector<AffordanceKit::AffordancePtr>& affordances = scene->getAffordances();
311 
312  for (auto& primitive : *primitives)
313  {
314  if (visualizationPrimitiveSet.size() > 0 && std::find(visualizationPrimitiveSet.begin(), visualizationPrimitiveSet.end(), primitive->getId()) == visualizationPrimitiveSet.end())
315  {
316  // This primitive is not configured to be visualized
317  continue;
318  }
319 
320  std::vector<std::string> labels;
321 
322  int i = 0;
323  for (auto& affordance : affordances)
324  {
325  if (visualizationAffordanceSet.size() > 0 && std::find(visualizationAffordanceSet.begin(), visualizationAffordanceSet.end(), affordance->getName()) == visualizationAffordanceSet.end())
326  {
327  // This affordance is not configured to be visualized
328  continue;
329  }
330 
331  if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceLabels && affordance->existsForPrimitive(primitive))
332  {
333  labels.push_back(affordance->getName());
334  }
335  else if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceBeliefFunctions)
336  {
337  visualizeAffordance(affordance, primitive, i++);
338  }
339  }
340 
341  if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceLabels)
342  {
343  visualizeAffordanceLabels(primitive, labels);
344  }
345  }
346 }
347 
348 void AffordancePipelineVisualization::visualizeAffordance(const AffordanceKit::AffordancePtr& affordance, const AffordanceKit::PrimitivePtr& primitive, unsigned int index)
349 {
350  std::string id = "affordance_" + affordance->getName() + "_" + std::to_string(index) + "_" + primitive->getId();
351 
352  if (affordance->getTimestamp() != primitive->getTimestamp())
353  {
354  IceUtil::Time affordanceTimestamp = IceUtil::Time::microSeconds(affordance->getTimestamp());
355  IceUtil::Time primitiveTimestamp = IceUtil::Time::microSeconds(primitive->getTimestamp());
356 
357  std::string t1 = affordanceTimestamp.toDateTime().substr(affordanceTimestamp.toDateTime().find(' ') + 1);
358  std::string t2 = primitiveTimestamp.toDateTime().substr(primitiveTimestamp.toDateTime().find(' ') + 1);
359 
360  ARMARX_WARNING << "Timestamp of visualized affordance does not match the primitive timestamp (Affordance: " << t1 << ", Primitive: " << t2 << ")";
361  }
362 
363  if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceBeliefFunctions)
364  {
365  if (auto unimanualA = std::dynamic_pointer_cast<AffordanceKit::UnimanualAffordance>(affordance))
366  {
369  }
370  else if (auto bimanualA = std::dynamic_pointer_cast<AffordanceKit::BimanualAffordance>(affordance))
371  {
374  }
375  }
376  else
377  {
378  ARMARX_WARNING << "Unknown affordance visualization style";
379  }
380 }
381 
382 void AffordancePipelineVisualization::visualizeAffordanceLabels(const AffordanceKit::PrimitivePtr& primitive, const std::vector<std::string>& labels)
383 {
384  std::string label;
385 
386  for (unsigned int i = 0; i < labels.size(); i++)
387  {
388  label += labels[i] + ((i == labels.size() - 1) ? "" : ", ");
389  }
390 
391  Eigen::Vector3f c = primitive->getCenter();
393 }
394 
396 {
397 }
398 
399 void AffordancePipelineVisualization::visualizePlane(const AffordanceKit::PlanePtr& plane)
400 {
401  const std::vector<Eigen::Vector3f>& hull = plane->getConvexHull();
402 
403  std::vector<Vector3BasePtr> polygon;
404  for (auto x : hull)
405  {
406  polygon.push_back(Vector3Ptr(new Vector3(x)));
407  }
408 
410 }
411 
412 void AffordancePipelineVisualization::visualizeSphere(const AffordanceKit::SpherePtr& sphere)
413 {
414  Vector3Ptr center(new Vector3(sphere->getCenter()));
415  debugDrawerTopicPrx->setSphereVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), sphere->getId(), center, primitiveColor, sphere->getRadius());
416 }
417 
418 void AffordancePipelineVisualization::visualizeCylinder(const AffordanceKit::CylinderPtr& cylinder)
419 {
420  float radius = cylinder->getRadius();
421  float length = cylinder->getLength();
422  Vector3Ptr point(new Vector3(cylinder->getBasePoint()));
423  Vector3Ptr direction(new Vector3(cylinder->getDirection()));
424 
425  debugDrawerTopicPrx->setCylinderVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), cylinder->getId(), point, direction, length, radius, primitiveColor);
426 }
427 
428 void AffordancePipelineVisualization::visualizeBox(const AffordanceKit::BoxPtr& box)
429 {
430  //debugDrawerTopicPrx->setBoxVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId(), new Pose(box->getPose()), new Vector3(box->getDimensions()), primitiveColor);
431 
432  Eigen::Matrix4f pose = box->getPose();
433  Eigen::Vector3f dim = box->getDimensions();
434 
435  std::vector<std::vector<Eigen::Vector3f>> sides;
436  sides.resize(6);
437 
438  sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
439  sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
440  sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
441  sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
442 
443  sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
444  sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
445  sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
446  sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
447 
448  sides[2].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
449  sides[2].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
450  sides[2].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
451  sides[2].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
452 
453  sides[3].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
454  sides[3].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
455  sides[3].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
456  sides[3].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
457 
458  sides[4].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
459  sides[4].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
460  sides[4].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
461  sides[4].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
462 
463  sides[5].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
464  sides[5].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
465  sides[5].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
466  sides[5].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
467 
468  for (unsigned int i = 0; i < sides.size(); i++)
469  {
470  debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_1", new Vector3(sides[i][0]), new Vector3(sides[i][1]), 3, primitiveColor);
471  debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_2", new Vector3(sides[i][1]), new Vector3(sides[i][2]), 3, primitiveColor);
472  debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_3", new Vector3(sides[i][2]), new Vector3(sides[i][3]), 3, primitiveColor);
473  debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_4", new Vector3(sides[i][3]), new Vector3(sides[i][0]), 3, primitiveColor);
474  }
475 }
476 
477 void AffordancePipelineVisualization::visualize(bool updatePrimitives, bool updateAffordances, bool updateDebugInfo)
478 {
479  sceneMutex.lock();
480 
481  if (!scene)
482  {
483  sceneMutex.unlock();
484  return;
485  }
486 
487  ARMARX_INFO << "Current scene contains: " << scene->getPrimitives()->size() << " primitives";
488  ARMARX_INFO << "Current scene contains: " << scene->getUnimanualAffordances().size() << " unimanual affordances";
489  ARMARX_INFO << "Current scene contains: " << scene->getBimanualAffordances().size() << " bimanual affordances";
490  ARMARX_INFO << "Current scene contains: " << scene->getAffordances().size() << " affordances in total";
491 
492  if (updatePrimitives)
493  {
494  ARMARX_INFO << "Visualizing primitives";
495 
498  }
499 
500  if (updateAffordances)
501  {
502  ARMARX_INFO << "Visualizing affordances";
503 
506  }
507 
508  if (updateDebugInfo)
509  {
510  ARMARX_INFO << "Visualizing debug information";
511 
516  }
517 
518  visualizeScene(scene, updatePrimitives, updateAffordances, updateDebugInfo);
519 
520  sceneMutex.unlock();
521 
522  // Update selections
523  if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
524  {
525  std::set<std::string> previousSelections = selections;
526 
528 
531 
532  for (auto& primitive : previousSelections)
533  {
534  ARMARX_INFO << "Selecting: " << primitive;
536  }
537  }
538 
539  affordanceVisualizationTopicPrx->reportAffordanceVisualizationUpdate();
540 }
541 
542 void AffordancePipelineVisualization::exportVisualizationAsIV(const std::string& filename, const Ice::Current& c)
543 {
544  // Currently there is no way to export multiple layers at once => exKport whole scene
545  ARMARX_INFO << "Exporting scene to '" << filename << "'";
546  debugDrawerTopicPrx->exportScene(filename);
547 }
548 
549 void AffordancePipelineVisualization::exportVisualizationAsJSON(const std::string& filename, const Ice::Current& c)
550 {
551  JSONObjectPtr serializer = new JSONObject(ElementType::eArray);
552 
553  memoryx::EnvironmentalPrimitiveBaseList primitives = environmentalPrimitiveSegment->getEnvironmentalPrimitives();
554 
555  for (auto& primitive : primitives)
556  {
557  JSONObjectPtr s = new JSONObject;
558 
559  if (primitive->ice_isA(memoryx::PlanePrimitiveBase::ice_staticId()))
560  {
561  memoryx::PlanePrimitiveBasePtr::dynamicCast(primitive)->serialize(s);
562  }
563  else if (primitive->ice_isA(memoryx::SpherePrimitiveBase::ice_staticId()))
564  {
565  memoryx::SpherePrimitiveBasePtr::dynamicCast(primitive)->serialize(s);
566  }
567  else if (primitive->ice_isA(memoryx::CylinderPrimitiveBase::ice_staticId()))
568  {
569  memoryx::CylinderPrimitiveBasePtr::dynamicCast(primitive)->serialize(s);
570  }
571  else
572  {
573  ARMARX_WARNING << "Could not serialize unknown primitive type";
574  }
575 
576  serializer->append(s);
577  }
578 
579  std::ofstream out;
580  out.open(filename, std::ios_base::trunc);
581 
582  if (out.is_open())
583  {
584  out << serializer->toString();
585  out.close();
586  ARMARX_INFO << "Scene exported as JSON to " << filename;
587  }
588  else
589  {
590  ARMARX_WARNING << "Failed to export scene as JSON to " << filename;
591  }
592 }
593 
595 {
596  ARMARX_INFO << "Clearing visualization";
597 
600 
603 
606 
609 
612 }
613 
614 void AffordancePipelineVisualization::reportSelectionChanged(const DebugDrawerSelectionList& selectedObjects, const Ice::Current&)
615 {
616  std::vector<std::string> tmpSelections;
617  selections.clear();
618 
619  ARMARX_INFO << "Selections cleared";
620 
621  for (auto& object : selectedObjects)
622  {
623  if (simox::alg::starts_with(object.layerName, "primitivesLayer"))
624  {
625  ARMARX_INFO << "Object " << object.elementName << " selected";
626 
627  selections.insert(object.elementName);
628  tmpSelections.push_back(object.elementName);
629  }
630  }
631 
632  affordanceVisualizationTopicPrx->reportPrimitiveSelectionChanged(tmpSelections);
633 }
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
EnvironmentalPrimitive.h
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
armarx::AffordancePipelineVisualization::selections
std::set< std::string > selections
Definition: AffordancePipelineVisualization.h:173
AffordancePipelineVisualization.h
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
alternatingPrimitiveVisualizationIndex
int alternatingPrimitiveVisualizationIndex
Definition: AffordancePipelineVisualization.cpp:55
armarx::AffordancePipelineVisualization::environmentalPrimitiveSegment
memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment
Definition: AffordancePipelineVisualization.h:171
armarx::ElementTypes::eArray
@ eArray
Definition: AbstractObjectSerializer.h:36
armarx::AffordancePipelineVisualization::onDisconnectComponent
void onDisconnectComponent() override
Definition: AffordancePipelineVisualization.cpp:153
armarx::JSONObject
The JSONObject class is used to represent and (de)serialize JSON objects.
Definition: JSONObject.h:43
armarx::AffordancePipelineVisualization::affordanceVisualizationTopicPrx
AffordancePipelineVisualizationListenerPrx affordanceVisualizationTopicPrx
Definition: AffordancePipelineVisualization.h:168
armarx::AffordancePipelineVisualization::deselectAllPrimitives
void deselectAllPrimitives(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:214
alternatingDebugInfoVisualizationIndex
int alternatingDebugInfoVisualizationIndex
Definition: AffordancePipelineVisualization.cpp:57
memoryx::AffordancePtr
IceInternal::Handle< Affordance > AffordancePtr
Definition: Affordance.h:40
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
PRIMITIVE_GRASP_POINTS_LAYER_NAME
#define PRIMITIVE_GRASP_POINTS_LAYER_NAME(n)
Definition: AffordancePipelineVisualization.cpp:52
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
armarx::AffordancePipelineVisualization::affordanceSegment
memoryx::AffordanceSegmentBasePrx affordanceSegment
Definition: AffordancePipelineVisualization.h:170
scene3D::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: PointerDefinitions.h:36
armarx::starts_with
bool starts_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:43
armarx::AffordancePipelineVisualization::visualizationAffordanceSet
std::vector< std::string > visualizationAffordanceSet
Definition: AffordancePipelineVisualization.h:189
Affordance.h
armarx::AffordancePipelineVisualization::scene
AffordanceKitArmarX::SceneArmarXPtr scene
Definition: AffordancePipelineVisualization.h:193
armarx::AffordancePipelineVisualization::primitiveColor
DrawColor primitiveColor
Definition: AffordancePipelineVisualization.h:177
AFFORDANCES_LAYER_NAME
#define AFFORDANCES_LAYER_NAME(n)
Definition: AffordancePipelineVisualization.cpp:53
IceInternal::Handle< Vector3 >
alternatingAffordanceVisualizationIndex
int alternatingAffordanceVisualizationIndex
Definition: AffordancePipelineVisualization.cpp:56
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::AffordancePipelineVisualization::exportVisualizationAsJSON
void exportVisualizationAsJSON(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:549
armarx::AffordancePipelineVisualization::debugDrawerTopicPrx
DebugDrawerInterfacePrx debugDrawerTopicPrx
Definition: AffordancePipelineVisualization.h:167
AffordanceKitArmarX::BimanualAffordanceArmarX
Definition: BimanualAffordanceArmarX.h:41
armarx::AffordancePipelineVisualization::onExitComponent
void onExitComponent() override
Definition: AffordancePipelineVisualization.cpp:157
armarx::AffordancePipelineVisualization::visualizeDebugInformation
void visualizeDebugInformation(const AffordanceKit::ScenePtr &scene)
Definition: AffordancePipelineVisualization.cpp:395
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::Vector3
The Vector3 class.
Definition: Pose.h:112
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
MemoryXCoreObjectFactories.h
AffordanceKitArmarX::SceneArmarX
Definition: SceneArmarX.h:33
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::AffordancePipelineVisualization::affordanceLabelSize
int affordanceLabelSize
Definition: AffordancePipelineVisualization.h:181
PRIMITIVE_FRAMES_LAYER_NAME
#define PRIMITIVE_FRAMES_LAYER_NAME(n)
Definition: AffordancePipelineVisualization.cpp:51
armarx::AffordancePipelineVisualization::AffordancePipelineVisualization
AffordancePipelineVisualization()
Definition: AffordancePipelineVisualization.cpp:61
armarx::AffordancePipelineVisualization::reportNewPointCloudSegmentation
void reportNewPointCloudSegmentation(const Ice::Current &c=Ice::emptyCurrent) override
Definition: AffordancePipelineVisualization.cpp:166
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
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::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::AffordancePipelineVisualizationPropertyDefinitions
Definition: AffordancePipelineVisualization.h:57
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
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
PRIMITIVE_LABELS_LAYER_NAME
#define PRIMITIVE_LABELS_LAYER_NAME(n)
Definition: AffordancePipelineVisualization.cpp:50
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
PRIMITIVE_LAYER_NAME
#define PRIMITIVE_LAYER_NAME(n)
Definition: AffordancePipelineVisualization.cpp:49
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
IceUtil::Handle< class PropertyDefinitionContainer >
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
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
UnimanualAffordanceArmarX.h
armarx::AffordancePipelineVisualization::onInitComponent
void onInitComponent() override
Definition: AffordancePipelineVisualization.cpp:70
MemoryXTypesObjectFactories.h
armarx::Vector3Ptr
IceInternal::Handle< Vector3 > Vector3Ptr
Definition: Pose.h:165
armarx::AffordancePipelineVisualization::affordanceLabelColor
DrawColor affordanceLabelColor
Definition: AffordancePipelineVisualization.h:179
AffordanceKitArmarX::UnimanualAffordanceArmarX
Definition: UnimanualAffordanceArmarX.h:37
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx::AffordancePipelineVisualization::visualizationPrimitiveSet
std::vector< std::string > visualizationPrimitiveSet
Definition: AffordancePipelineVisualization.h:188
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
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
BimanualAffordanceArmarX.h