DebugDrawerComponent.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotAPI::ArmarXObjects::DebugDrawerComponent
19  * @author Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
20  * @author Peter Kaiser ( peter dot kaiser at kit dot edu )
21  * @date 2014
22  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
23  * GNU General Public License
24  */
25 
26 #include "DebugDrawerComponent.h"
27 
28 #include <SimoxUtility/algorithm/string/string_tools.h>
29 #include <VirtualRobot/Robot.h>
30 #include <VirtualRobot/VirtualRobot.h>
31 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
32 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
33 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
34 #include <VirtualRobot/Visualization/TriMeshModel.h>
35 #include <VirtualRobot/Visualization/VisualizationFactory.h>
36 #include <VirtualRobot/XML/RobotIO.h>
37 
38 #include <ArmarXCore/core/system/ArmarXDataPath.h>
39 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
40 
41 #include <RobotAPI/libraries/core/math/ColorUtils.h>
42 
43 #include <Inventor/SbVec3f.h>
44 #include <Inventor/actions/SoWriteAction.h>
45 #include <Inventor/fields/SoMFColor.h>
46 #include <Inventor/fields/SoMFVec3f.h>
47 #include <Inventor/nodes/SoAnnotation.h>
48 #include <Inventor/nodes/SoComplexity.h>
49 #include <Inventor/nodes/SoCoordinate3.h>
50 #include <Inventor/nodes/SoCube.h>
51 #include <Inventor/nodes/SoCylinder.h>
52 #include <Inventor/nodes/SoDrawStyle.h>
53 #include <Inventor/nodes/SoFont.h>
54 #include <Inventor/nodes/SoLineSet.h>
55 #include <Inventor/nodes/SoMaterial.h>
56 #include <Inventor/nodes/SoMaterialBinding.h>
57 #include <Inventor/nodes/SoMatrixTransform.h>
58 #include <Inventor/nodes/SoPointSet.h>
59 #include <Inventor/nodes/SoShapeHints.h>
60 #include <Inventor/nodes/SoSphere.h>
61 #include <Inventor/nodes/SoText2.h>
62 #include <Inventor/nodes/SoTransform.h>
63 #include <Inventor/nodes/SoTranslation.h>
64 #include <Inventor/nodes/SoUnits.h>
65 
66 using namespace VirtualRobot;
67 
68 #define SELECTION_NAME_PREFIX \
69  ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
70 #define SELECTION_NAME_SPLITTER "____"
71 #define SELECTION_NAME(layerName, elementName) \
72  simox::alg::replace_all(std::string(SELECTION_NAME_PREFIX + "_" + layerName + \
73  SELECTION_NAME_SPLITTER + elementName), \
74  " ", \
75  "_") \
76  .c_str()
77 
78 namespace armarx
79 {
80  std::recursive_mutex DebugDrawerComponent::selectionMutex;
81 
82  void
83  selection_callback(void* userdata, SoPath* path)
84  {
85  if (userdata)
86  {
87  ((DebugDrawerComponent*)userdata)->selectionCallback();
88  }
89  }
90 
91  void
92  deselection_callback(void* userdata, SoPath* path)
93  {
94  if (userdata)
95  {
96  ((DebugDrawerComponent*)userdata)->deselectionCallback();
97  }
98  }
99 
100  static const std::string DEBUG_LAYER_NAME{"debug"};
101 
102  DebugDrawerComponent::DebugDrawerComponent() :
103  mutex(new RecursiveMutex()),
104  dataUpdateMutex(new RecursiveMutex()),
105  topicMutex(new RecursiveMutex()),
106  defaultLayerVisibility(true)
107  {
108  cycleTimeMS = 1000.0f / 30.0f;
109  timerSensor = NULL;
110  verbose = true;
111 
112  coinVisu = new SoSeparator;
113  coinVisu->ref();
114 
115  selectionNode = new SoSelection;
116  selectionNode->policy = SoSelection::SHIFT;
117  selectionNode->ref();
118  coinVisu->addChild(selectionNode);
119 
120  layerMainNode = new SoSeparator;
121  layerMainNode->ref();
122 
123  SoUnits* u = new SoUnits();
124  u->units = SoUnits::MILLIMETERS;
125  selectionNode->addChild(u);
126  selectionNode->addChild(layerMainNode);
127  }
128 
129  void
130  DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
131  {
132  auto l = getScopedVisuLock();
133 
134  ARMARX_INFO << "Enabling selections for layer " << layerName;
135  selectableLayers.insert(layerName);
136  }
137 
138  void
139  DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
140  {
141  auto l = getScopedVisuLock();
142 
143  ARMARX_INFO << "Disabling selections for layer " << layerName;
144  if (layerName == "")
145  {
146  selectableLayers.clear();
147  }
148  else if (layers.find(layerName) != layers.end())
149  {
150  selectableLayers.erase(layerName);
151  }
152  }
153 
154  void
155  DebugDrawerComponent::clearSelections(const std::string& layerName, const Ice::Current&)
156  {
157  auto l = getScopedVisuLock();
158  std::unique_lock l2(selectionMutex);
159 
160  removeSelectionCallbacks();
161 
162  ARMARX_INFO << "Clearing selections on layer " << layerName;
163  if (layerName == "")
164  {
165  selectionNode->deselectAll();
166  }
167  else if (layers.find(layerName) != layers.end())
168  {
169  for (int i = 0; i < selectionNode->getNumSelected(); i++)
170  {
171  SoPath* path = selectionNode->getPath(i);
172  if (path->containsNode(layers.at(layerName).mainNode))
173  {
174  selectionNode->deselect(path);
175  }
176  }
177  }
178 
179  installSelectionCallbacks();
180  }
181 
182  void
183  DebugDrawerComponent::select(const std::string& layerName,
184  const std::string& elementName,
185  const Ice::Current&)
186  {
187  auto l = getScopedVisuLock();
188  std::unique_lock l2(selectionMutex);
189 
190  removeSelectionCallbacks();
191 
192  ARMARX_DEBUG << "Selecting element '" << elementName << "' on layer '" << layerName
193  << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
194 
195  SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
196  if (n)
197  {
198  selectionNode->select(n);
199  }
200  else
201  {
202  ARMARX_WARNING << "Element to select not found";
203  }
204 
205  // Force visualization update
206  selectionNode->touch();
207 
208  installSelectionCallbacks();
209  }
210 
211  void
212  DebugDrawerComponent::deselect(const std::string& layerName,
213  const std::string& elementName,
214  const Ice::Current&)
215  {
216  auto l = getScopedVisuLock();
217  std::unique_lock l2(selectionMutex);
218 
219  removeSelectionCallbacks();
220 
221  ARMARX_DEBUG << "Deselecting element '" << elementName << "' on layer '" << layerName
222  << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
223 
224  SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
225  if (n)
226  {
227  selectionNode->deselect(n);
228  }
229  else
230  {
231  ARMARX_WARNING << "Element to deselect not found";
232  }
233 
234  // Force visualization update
235  selectionNode->touch();
236 
237  installSelectionCallbacks();
238  }
239 
240  void
241  DebugDrawerComponent::selectionCallback()
242  {
243  std::unique_lock l(*topicMutex);
244  listenerPrx->reportSelectionChanged(getSelections());
245  }
246 
247  void
248  DebugDrawerComponent::deselectionCallback()
249  {
250  std::unique_lock l(*topicMutex);
251  listenerPrx->reportSelectionChanged(getSelections());
252  }
253 
254  void
255  DebugDrawerComponent::installSelectionCallbacks()
256  {
257  auto l = getScopedVisuLock();
258  selectionNode->addSelectionCallback(selection_callback, this);
259  selectionNode->addDeselectionCallback(deselection_callback, this);
260  }
261 
262  void
263  DebugDrawerComponent::removeSelectionCallbacks()
264  {
265  auto l = getScopedVisuLock();
266  selectionNode->removeSelectionCallback(selection_callback, this);
267  selectionNode->removeDeselectionCallback(deselection_callback, this);
268  }
269 
270  void
271  DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements,
272  const Ice::Current&)
273  {
274  auto l = getScopedVisuLock();
275  std::unique_lock l2(selectionMutex);
276 
277  removeSelectionCallbacks();
278  selectionNode->deselectAll();
279 
280  for (auto& e : selectedElements)
281  {
282  SoNode* n = SoSelection::getByName(SELECTION_NAME(e.layerName, e.elementName));
283  if (n)
284  {
285  selectionNode->select(n);
286  }
287  }
288 
289  // Force visualization update
290  selectionNode->touch();
291 
292  installSelectionCallbacks();
293  }
294 
295  DebugDrawerSelectionList
296  DebugDrawerComponent::getSelections(const ::Ice::Current&)
297  {
298  auto l = getScopedVisuLock();
299 
300  DebugDrawerSelectionList selectedElements;
301 
302  for (int i = 0; i < selectionNode->getNumSelected(); i++)
303  {
304  SoPath* path = selectionNode->getPath(i);
305 
306  for (int j = 0; j < path->getLength(); j++)
307  {
308  SoNode* node = path->getNodeFromTail(j);
309  if (!node)
310  {
311  continue;
312  }
313 
314  std::string name = node->getName().getString();
315  if (name.length() > 0 && name.find(SELECTION_NAME_PREFIX) == 0)
316  {
317  int index = name.rfind(SELECTION_NAME_SPLITTER);
318  name = name.substr(index + strlen(SELECTION_NAME_SPLITTER));
319 
320  // Check if selected element is 'selectable'
321  for (auto& l : layers)
322  {
323  std::string layer = l.first;
324  if (layers[layer].addedBoxVisualizations.find(name) !=
325  layers[layer].addedBoxVisualizations.end() ||
326  layers[layer].addedTextVisualizations.find(name) !=
327  layers[layer].addedTextVisualizations.end() ||
328  layers[layer].addedSphereVisualizations.find(name) !=
329  layers[layer].addedSphereVisualizations.end() ||
330  layers[layer].addedCylinderVisualizations.find(name) !=
331  layers[layer].addedCylinderVisualizations.end() ||
332  layers[layer].addedPolygonVisualizations.find(name) !=
333  layers[layer].addedPolygonVisualizations.end())
334  {
335  DebugDrawerSelectionElement e;
336  e.layerName = layer;
337  e.elementName = name;
338 
339  selectedElements.push_back(e);
340  break;
341  }
342  }
343 
344  // We only process the first node that matches the naming scheme
345  break;
346  }
347  }
348  }
349 
350  return selectedElements;
351  }
352 
353  void
354  DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
355  {
356  if (timerSensor)
357  {
358  ARMARX_ERROR << "Cannot change the cycle time, once the thread has been started...";
359  return;
360  }
361 
362  if (visuUpdatesPerSec <= 0)
363  {
364  cycleTimeMS = 0;
365  }
366  else
367  {
368  cycleTimeMS = 1000.0f / (float)visuUpdatesPerSec;
369  }
370  }
371 
372  DebugDrawerComponent::~DebugDrawerComponent()
373  {
374  /*{
375  auto l = getScopedLock();
376  layerMainNode->unref();
377  coinVisu->unref();
378  }*/
379  }
380 
381  void
382  DebugDrawerComponent::onInitComponent()
383  {
384  usingTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
385  bool enabled = getProperty<bool>("ShowDebugDrawing").getValue();
386 
387  if (!enabled)
388  {
389  disableAllLayers();
390  }
391 
392  if (cycleTimeMS > 0)
393  {
394  SoSensorManager* sensor_mgr = SoDB::getSensorManager();
395  timerSensor = new SoTimerSensor(updateVisualizationCB, this);
396  timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
397  sensor_mgr->insertTimerSensor(timerSensor);
398 
399  /*execTaskVisuUpdates = new PeriodicTask<DebugDrawerComponent>(this, &DebugDrawerComponent::updateVisualization, cycleTimeMS, false, "DebugDrawerComponentVisuUpdates");
400  execTaskVisuUpdates->start();
401  execTaskVisuUpdates->setDelayWarningTolerance(100);*/
402  }
403 
404  offeringTopic(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
405  usingTopic(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
406 
407  installSelectionCallbacks();
408  }
409 
410  void
411  DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
412  {
413  DebugDrawerComponent* drawer = static_cast<DebugDrawerComponent*>(data);
414 
415  if (!drawer)
416  {
417  return;
418  }
419 
420  std::unique_lock lock(drawer->timerSensorMutex);
421  drawer->updateVisualization();
422  }
423 
424  void
425  DebugDrawerComponent::onConnectComponent()
426  {
427  verbose = true;
428 
429  listenerPrx = getTopic<DebugDrawerListenerPrx>(
430  getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
431  }
432 
433  void
434  DebugDrawerComponent::onDisconnectComponent()
435  {
436  std::cout << "onDisconnectComponent debug drawer" << std::endl;
437  /*verbose = false;
438  size_t c = layers.size();
439  size_t counter = 0;
440  while (layers.size()>0 && counter < 2*c)
441  {
442  std::cout << "removing layer " << layers.begin()->first;
443  removeLayer(layers.begin()->first);
444  counter++; // sec counter
445  }
446  {
447  auto l = getScopedLock();
448  coinVisu->removeAllChildren();
449  }*/
450  }
451 
452  void
453  DebugDrawerComponent::onExitComponent()
454  {
455  ARMARX_INFO << "onExitComponent debug drawer";
456  verbose = false;
457 
458  //ARMARX_DEBUG << "onExitComponent";
459  if (timerSensor)
460  {
461  std::unique_lock lock(timerSensorMutex);
462  SoSensorManager* sensor_mgr = SoDB::getSensorManager();
463  sensor_mgr->removeTimerSensor(timerSensor);
464  }
465 
466  size_t c = layers.size();
467  size_t counter = 0;
468 
469  while (layers.size() > 0 && counter < 2 * c)
470  {
471  ARMARX_INFO << "removing layer " << layers.begin()->first;
472  removeLayer(layers.begin()->first);
473  counter++; // sec counter
474  }
475 
476  {
477  auto l = getScopedVisuLock();
478  coinVisu->removeAllChildren();
479  layerMainNode->unref();
480  selectionNode->unref();
481  coinVisu->unref();
482  }
483  }
484 
485  void
486  DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
487  {
488  auto l = getScopedVisuLock();
489 
490  ARMARX_INFO << "Exporting scene to '" << filename << "'";
491 
492  SoSeparator* sep = new SoSeparator;
493  SoUnits* u = new SoUnits;
494  u->units = SoUnits::MILLIMETERS;
495  sep->addChild(u);
496  sep->addChild(layerMainNode);
497 
498  SoWriteAction writeAction;
499  writeAction.getOutput()->openFile(filename.c_str());
500  writeAction.getOutput()->setBinary(false);
501  writeAction.apply(sep);
502  writeAction.getOutput()->closeFile();
503 
504  sep->unref();
505  }
506 
507  void
508  DebugDrawerComponent::exportLayer(const std::string& filename,
509  const std::string& layerName,
510  const Ice::Current&)
511  {
512  auto l = getScopedVisuLock();
513 
514  ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
515 
516  if (!hasLayer(layerName))
517  {
518  ARMARX_WARNING << "Unknown layer to export";
519  return;
520  }
521 
522  SoSeparator* sep = new SoSeparator;
523  SoUnits* u = new SoUnits;
524  u->units = SoUnits::MILLIMETERS;
525  sep->addChild(u);
526  sep->addChild(layers[layerName].mainNode);
527 
528  SoWriteAction writeAction;
529  writeAction.getOutput()->openFile(filename.c_str());
530  writeAction.getOutput()->setBinary(false);
531  writeAction.apply(sep);
532  writeAction.getOutput()->closeFile();
533 
534  sep->unref();
535  }
536 
537  void
538  DebugDrawerComponent::drawCoordSystem(const CoordData& d)
539  {
540  auto l = getScopedVisuLock();
541  ARMARX_DEBUG << "drawing coord system";
542 
543  auto& layer = requestLayer(d.layerName);
544 
545  removeCoordSystem(d.layerName, d.name);
546 
547  if (d.scale <= 0 || !d.active)
548  {
549  return;
550  }
551 
552  //if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
553  //{
554  SoSeparator* newS = new SoSeparator;
555  SoMatrixTransform* newM = new SoMatrixTransform;
556  newS->addChild(newM);
557  std::string n = d.name;
558  newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(d.scale, &n));
559  layer.addedCoordVisualizations[d.name] = newS;
560  layer.mainNode->addChild(newS);
561  //}
562  SoSeparator* s = layer.addedCoordVisualizations[d.name];
563  SoMatrixTransform* m = (SoMatrixTransform*)(s->getChild(0));
564  SbMatrix mNew = CoinVisualizationFactory::getSbMatrix(d.globalPose);
565  m->matrix.setValue(mNew);
566 
567  ARMARX_DEBUG << "end";
568  }
569 
570  void
571  DebugDrawerComponent::drawLine(const LineData& d)
572  {
573  auto l = getScopedVisuLock();
574  ARMARX_DEBUG << "drawLine1" << flush;
575 
576  auto& layer = requestLayer(d.layerName);
577 
578  removeLine(d.layerName, d.name);
579 
580  if (!d.active)
581  {
582  return;
583  }
584 
585  SoSeparator* newS = new SoSeparator;
586  newS->addChild(CoinVisualizationFactory::createCoinLine(d.p1, d.p2, d.scale, d.color));
587  layer.addedLineVisualizations[d.name] = newS;
588  layer.mainNode->addChild(newS);
589  ARMARX_DEBUG << "drawLine2" << flush;
590  }
591 
592  void
593  DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
594  {
595  auto l = getScopedVisuLock();
596  ARMARX_DEBUG << "drawLineSet";
597 
598  auto& layer = requestLayer(d.layerName);
599 
600  removeLineSet(d.layerName, d.name);
601 
602  if (!d.active)
603  {
604  return;
605  }
606 
607  if (d.lineSet.points.size() % 2 != 0)
608  {
609  ARMARX_WARNING << "A line set requires an even amount of points";
610  return;
611  }
612 
613  if (d.lineSet.points.size() != 2 * d.lineSet.intensities.size())
614  {
615  ARMARX_WARNING
616  << "Amount of intensities has to be half the amount of points for a line set";
617  return;
618  }
619 
620  for (const DebugDrawerPointCloudElement& e : d.lineSet.points)
621  {
622  if (!std::isfinite(e.x) || !std::isfinite(e.y) || !std::isfinite(e.z))
623  {
624  ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A point of lineset "
625  << d.name << " on layer " << d.layerName
626  << " is not finite! this set will not be drawn!";
627  return;
628  }
629  }
630 
631  // Initialize color map for affordance visualization
632  std::vector<VirtualRobot::VisualizationFactory::Color> colors;
633  colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorNoIntensity.r,
634  d.lineSet.colorNoIntensity.g,
635  d.lineSet.colorNoIntensity.b));
636  colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorFullIntensity.r,
637  d.lineSet.colorFullIntensity.g,
638  d.lineSet.colorFullIntensity.b));
639  VirtualRobot::ColorMap visualizationColorMap =
640  VirtualRobot::ColorMap::customColorMap(colors);
641 
642  // Initialize visualization nodes
643  SoSeparator* sep = new SoSeparator;
644 
645  SoMaterialBinding* binding = new SoMaterialBinding;
646  binding->value = SoMaterialBinding::PER_PART;
647  sep->addChild(binding);
648 
649  SoDrawStyle* lineStyle = new SoDrawStyle;
650  lineStyle->lineWidth.setValue(d.lineSet.lineWidth);
651  sep->addChild(lineStyle);
652 
653  SoCoordinate3* coordinateNode = new SoCoordinate3;
654  sep->addChild(coordinateNode);
655 
656  SoMaterial* materialNode = new SoMaterial;
657  sep->addChild(materialNode);
658 
659  SoLineSet* lineSetNode = new SoLineSet;
660  lineSetNode->startIndex.setValue(0);
661  sep->addChild(lineSetNode);
662 
663 
664  // Allocate index and material arrays
665  SbVec3f* coordinateValues = new SbVec3f[d.lineSet.points.size()];
666  int32_t* lineSetValues = new int32_t[d.lineSet.intensities.size()];
667  SbColor* colorValues = new SbColor[d.lineSet.intensities.size()];
668 
669  for (unsigned int i = 0; i < d.lineSet.intensities.size(); i++)
670  {
671  lineSetValues[i] = 2;
672 
673  coordinateValues[2 * i].setValue(
674  d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
675  coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x,
676  d.lineSet.points[2 * i + 1].y,
677  d.lineSet.points[2 * i + 1].z);
678 
679  if (d.lineSet.useHeatMap)
680  {
681  auto c = colorutils::HeatMapRGBAColor(d.lineSet.intensities[i]);
682  colorValues[i].setValue(c.r, c.g, c.b);
683  }
684  else
685  {
686  VirtualRobot::VisualizationFactory::Color c =
687  visualizationColorMap.getColor(d.lineSet.intensities[i]);
688  colorValues[i].setValue(c.r, c.g, c.b);
689  }
690  }
691 
692  coordinateNode->point.setValuesPointer(d.lineSet.points.size(), coordinateValues);
693  lineSetNode->numVertices.setValuesPointer(d.lineSet.intensities.size(), lineSetValues);
694  materialNode->ambientColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
695  materialNode->diffuseColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
696 
697  layer.addedLineSetVisualizations[d.name] = sep;
698  layer.mainNode->addChild(sep);
699  }
700 
701  void
702  DebugDrawerComponent::drawBox(const BoxData& d)
703  {
704  auto l = getScopedVisuLock();
705  ARMARX_DEBUG << "drawBox";
706 
707  auto& layer = requestLayer(d.layerName);
708 
709  removeBox(d.layerName, d.name);
710 
711  if (!d.active)
712  {
713  return;
714  }
715 
716  SoSeparator* newS = new SoSeparator;
717  Eigen::Matrix4f m = d.globalPose;
718  newS->addChild(CoinVisualizationFactory::getMatrixTransform(m));
719 
720  SoMaterial* material = new SoMaterial;
721  material->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
722  material->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
723  material->transparency.setValue(d.color.transparency);
724  newS->addChild(material);
725 
726  SoCube* cube = new SoCube;
727  cube->setName(SELECTION_NAME(d.layerName, d.name));
728  cube->width = d.width;
729  cube->height = d.height;
730  cube->depth = d.depth;
731  newS->addChild(cube);
732 
733  layer.addedBoxVisualizations[d.name] = newS;
734  layer.mainNode->addChild(newS);
735  }
736 
737  void
738  DebugDrawerComponent::drawText(const TextData& d)
739  {
740  auto l = getScopedVisuLock();
741  ARMARX_DEBUG << "drawText1";
742 
743  auto& layer = requestLayer(d.layerName);
744 
745  removeText(d.layerName, d.name);
746 
747  if (!d.active)
748  {
749  return;
750  }
751 
752  SoSeparator* sep = new SoSeparator;
753  SoAnnotation* ann = new SoAnnotation;
754  sep->addChild(ann);
755 
756  SoMaterial* mat = new SoMaterial;
757  mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
758  mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
759  mat->transparency.setValue(d.color.transparency);
760  ann->addChild(mat);
761 
762  SoTransform* tr = new SoTransform;
763  tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
764  ann->addChild(tr);
765 
766  SoFont* font = new SoFont;
767  font->name.setValue("TGS_Triplex_Roman");
768  font->size = d.size;
769 
770  ann->addChild(font);
771 
772  SoText2* te = new SoText2;
773  te->setName(SELECTION_NAME(d.layerName, d.name));
774  te->string = d.text.c_str();
775  te->justification = SoText2::CENTER;
776  ann->addChild(te);
777 
778  layer.addedTextVisualizations[d.name] = sep;
779  layer.mainNode->addChild(sep);
780  ARMARX_DEBUG << "drawText2";
781  }
782 
783  void
784  DebugDrawerComponent::drawSphere(const SphereData& d)
785  {
786  auto l = getScopedVisuLock();
787  ARMARX_DEBUG << "drawSphere";
788 
789  auto& layer = requestLayer(d.layerName);
790 
791  removeSphere(d.layerName, d.name);
792 
793  if (!d.active)
794  {
795  return;
796  }
797 
798  if (!std::isfinite(d.position(0)) || !std::isfinite(d.position(1)) ||
799  !std::isfinite(d.position(2)))
800  {
801  ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A coordinate of sphere "
802  << d.name << " on layer " << d.layerName
803  << " is not finite! this sphere will not be drawn!";
804  return;
805  }
806 
807  SoSeparator* sep = new SoSeparator;
808  SoMaterial* mat = new SoMaterial;
809  mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
810  mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
811  mat->transparency.setValue(d.color.transparency);
812  sep->addChild(mat);
813 
814  SoTransform* tr = new SoTransform;
815  tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
816  sep->addChild(tr);
817 
818  SoSphere* sphere = new SoSphere;
819  sphere->setName(SELECTION_NAME(d.layerName, d.name));
820  sphere->radius = d.radius;
821  sep->addChild(sphere);
822 
823  layer.addedSphereVisualizations[d.name] = sep;
824  layer.mainNode->addChild(sep);
825  }
826 
827  void
828  DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
829  {
830  auto l = getScopedVisuLock();
831  ARMARX_DEBUG << "drawCylinder";
832 
833  auto& layer = requestLayer(d.layerName);
834 
835  removeCylinder(d.layerName, d.name);
836 
837  if (!d.active)
838  {
839  return;
840  }
841 
842  SoSeparator* sep = new SoSeparator;
843  SoMaterial* mat = new SoMaterial;
844  mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
845  mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
846  mat->transparency.setValue(d.color.transparency);
847  sep->addChild(mat);
848 
849  // The cylinder extends in y-direction, hence choose the transformation
850  SoTransform* tr = new SoTransform;
851  tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
852  tr->rotation.setValue(SbRotation(
853  SbVec3f(0, 1, 0), SbVec3f(d.direction.x(), d.direction.y(), d.direction.z())));
854  sep->addChild(tr);
855 
856  SoCylinder* cylinder = new SoCylinder;
857  cylinder->setName(SELECTION_NAME(d.layerName, d.name));
858  cylinder->height = d.length;
859  cylinder->radius = d.radius;
860  sep->addChild(cylinder);
861 
862  layer.addedCylinderVisualizations[d.name] = sep;
863  layer.mainNode->addChild(sep);
864  }
865 
866  Eigen::Vector3f
867  GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f& dir1, Eigen::Vector3f& dir2)
868  {
869 
870  vec = vec.normalized();
871 
872  dir1 = vec.cross(Eigen::Vector3f(0, 0, 1));
873 
874  if (dir1.norm() < 0.1f)
875 
876  {
877 
878  dir1 = vec.cross(Eigen::Vector3f(0, 1, 0));
879  }
880 
881  dir1 = -dir1.normalized();
882 
883  dir2 = vec.cross(dir1).normalized();
884 
885  return vec;
886  }
887 
888  void
889  DebugDrawerComponent::drawCircle(const DebugDrawerComponent::CircleData& d)
890  {
891  auto l = getScopedVisuLock();
892 
893  auto& layer = requestLayer(d.layerName);
894 
895  removeCircle(d.layerName, d.name);
896 
897  if (!d.active)
898  {
899  return;
900  }
901 
902 
903  SoSeparator* sep = new SoSeparator;
904  SoMaterial* mat = new SoMaterial;
905  mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
906  mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
907  mat->transparency.setValue(d.color.transparency);
908  sep->addChild(mat);
909 
910  // The circle extends in x-y-plane, hence choose the transformation
911  SoTransform* tr = new SoTransform;
912  tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
913  Eigen::Vector3f xDir, yDir;
914  Eigen::Vector3f zDir = GetOrthonormalVectors(d.direction, xDir, yDir);
915  Eigen::Matrix3f rotGoal;
916  rotGoal << xDir, yDir, zDir;
917  // auto quat = VirtualRobot::MathTools::getRotation(Eigen::Vector3f::UnitZ(), d.direction);
918  // Eigen::AngleAxisf rotAA(Eigen::Quaternionf(quat.w, quat.x, quat.y, quat.z));
919  Eigen::AngleAxisf rotAA(rotGoal);
920  tr->rotation.setValue(SbVec3f(rotAA.axis().x(), rotAA.axis().y(), rotAA.axis().z()),
921  rotAA.angle());
922  sep->addChild(tr);
923  auto node = CoinVisualizationFactory().createCircleArrow(
924  d.radius, d.width, d.circleCompletion, d.color, 16, 30);
925  SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
926  circle->setName(SELECTION_NAME(d.layerName, d.name));
927  sep->addChild(circle);
928 
929  layer.addedCircleVisualizations[d.name] = sep;
930  layer.mainNode->addChild(sep);
931  }
932 
933  void
934  DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
935  {
936  auto l = getScopedVisuLock();
937 
938  auto& layer = requestLayer(d.layerName);
939 
940  removePointCloud(d.layerName, d.name);
941 
942  if (!d.active)
943  {
944  return;
945  }
946 
947  const auto& pcl = d.pointCloud.points;
948 
949  SoSeparator* pclSep = new SoSeparator;
950 
951  SoMaterial* pclMat = new SoMaterial;
952  pclMat->diffuseColor.setValue(0.2, 0.2, 0.2);
953  pclMat->diffuseColor.setValue(0.2, 0.2, 0.2);
954  pclSep->addChild(pclMat);
955 
956  SoMaterialBinding* pclMatBind = new SoMaterialBinding;
957  pclMatBind->value = SoMaterialBinding::OVERALL;
958  pclSep->addChild(pclMatBind);
959 
960  SoCoordinate3* pclCoords = new SoCoordinate3;
961  std::vector<SbVec3f> coords;
962  coords.reserve(pcl.size());
963  std::transform(pcl.begin(),
964  pcl.end(),
965  std::back_inserter(coords),
966  [](const DebugDrawerPointCloudElement& elem) {
967  return SbVec3f{elem.x, elem.y, elem.z};
968  });
969  pclCoords->point.setValues(0, coords.size(), coords.data());
970  pclSep->addChild(pclCoords);
971 
972  SoDrawStyle* pclStye = new SoDrawStyle;
973  pclStye->pointSize = d.pointCloud.pointSize;
974  pclSep->addChild(pclStye);
975 
976  pclSep->addChild(new SoPointSet);
977 
978  layer.addedPointCloudVisualizations[d.name] = pclSep;
979  layer.mainNode->addChild(pclSep);
980  }
981 
982  void
983  DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
984  {
985  auto l = getScopedVisuLock();
986 
987  auto& layer = requestLayer(d.layerName);
988 
989  removePolygon(d.layerName, d.name);
990 
991  if (!d.active)
992  {
993  return;
994  }
995 
996  SoSeparator* sep = new SoSeparator;
997 
998  SoShapeHints* hints = new SoShapeHints;
999  hints->vertexOrdering = SoShapeHints::COUNTERCLOCKWISE;
1000  hints->shapeType = SoShapeHints::UNKNOWN_SHAPE_TYPE;
1001  hints->faceType = SoShapeHints::UNKNOWN_FACE_TYPE;
1002  sep->addChild(hints);
1003 
1004  sep->addChild(VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(
1005  d.points, d.colorInner, d.colorBorder, d.lineWidth));
1006  sep->setName(SELECTION_NAME(d.layerName, d.name));
1007  layer.addedPolygonVisualizations[d.name] = sep;
1008  layer.mainNode->addChild(sep);
1009  }
1010 
1011  void
1012  DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
1013  {
1014  auto l = getScopedVisuLock();
1015 
1016  auto& layer = requestLayer(d.layerName);
1017 
1018  removeTriMesh(d.layerName, d.name);
1019 
1020  if (!d.active)
1021  {
1022  return;
1023  }
1024 
1025  TriMeshModelPtr triMesh = TriMeshModelPtr(new TriMeshModel());
1026 
1027  for (DrawColor color : d.triMesh.colors)
1028  {
1029  triMesh->addColor(
1030  VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
1031  }
1032 
1033  for (DebugDrawerVertex v : d.triMesh.vertices)
1034  {
1035  triMesh->addVertex(Eigen::Vector3f(v.x, v.y, v.z));
1036  }
1037 
1038  for (DebugDrawerFace f : d.triMesh.faces)
1039  {
1040  MathTools::TriangleFace face;
1041 
1042  face.id1 = f.vertex1.vertexID;
1043  face.idNormal1 = f.vertex1.normalID;
1044  face.idColor1 = f.vertex1.colorID;
1045 
1046  face.id2 = f.vertex2.vertexID;
1047  face.idNormal2 = f.vertex2.normalID;
1048  face.idColor2 = f.vertex2.colorID;
1049 
1050  face.id3 = f.vertex3.vertexID;
1051  face.idNormal3 = f.vertex3.normalID;
1052  face.idColor3 = f.vertex3.colorID;
1053 
1054  face.normal = Eigen::Vector3f(f.normal.x, f.normal.y, f.normal.z);
1055 
1056  triMesh->addFace(face);
1057  }
1058 
1059  SoSeparator* sep = new SoSeparator();
1060  SoNode* c = CoinVisualizationFactory::getCoinVisualization(triMesh);
1061  sep->addChild(c);
1062  layer.addedTriMeshVisualizations[d.name] = sep;
1063  layer.mainNode->addChild(sep);
1064  }
1065 
1066  void
1067  DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
1068  {
1069  auto l = getScopedVisuLock();
1070 
1071  auto& layer = requestLayer(d.layerName);
1072 
1073  removeArrow(d.layerName, d.name);
1074 
1075  if (!d.active)
1076  {
1077  return;
1078  }
1079 
1080  SoSeparator* sep = new SoSeparator;
1081 
1082  SoTransform* tr = new SoTransform;
1083  tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
1084  sep->addChild(tr);
1085 
1086  SoSeparator* sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(
1087  d.direction, d.length, d.width, d.color);
1088  sep->addChild(sepArrow);
1089  layer.addedArrowVisualizations[d.name] = sep;
1090  layer.mainNode->addChild(sep);
1091  }
1092 
1093  void
1094  DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
1095  {
1096  auto l = getScopedVisuLock();
1097 
1098  if (!d.active)
1099  {
1100  return;
1101  }
1102 
1103  // load or get robot
1104  RobotPtr rob = requestRobot(d);
1105 
1106  if (!rob)
1107  {
1108  ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name
1109  << " at layer " << d.layerName;
1110  return;
1111  }
1112 
1113  std::map<std::string, float>::iterator i = d.configuration.begin();
1114  while (i != d.configuration.end())
1115  {
1116  if (!rob->hasRobotNode(i->first))
1117  {
1118  ARMARX_WARNING << deactivateSpam() << "Robot " << rob->getName()
1119  << " does not know RobotNode " << i->first;
1120  i = d.configuration.erase(i);
1121  }
1122  else
1123  {
1124  i++;
1125  }
1126  }
1127  }
1128 
1129  void
1130  DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
1131  {
1132  auto l = getScopedVisuLock();
1133 
1134  auto& layer = requestLayer(d.layerName);
1135 
1136  if (!d.active)
1137  {
1138  removeRobot(d.layerName, d.name);
1139  return;
1140  }
1141 
1142  // load or get robot
1143  RobotPtr rob = requestRobot(d);
1144 
1145  if (!rob)
1146  {
1147  ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name
1148  << " at layer " << d.layerName;
1149  return;
1150  }
1151 
1152  if (d.updatePose)
1153  {
1154  rob->setGlobalPose(d.globalPose);
1155  }
1156 
1157  if (d.updateConfig)
1158  {
1159  rob->setJointValues(d.configuration);
1160  }
1161 
1162  if (d.updateColor)
1163  {
1164  if (layer.addedRobotVisualizations.find(d.name) == layer.addedRobotVisualizations.end())
1165  {
1166  ARMARX_WARNING << deactivateSpam() << "Internal robot visu error";
1167  }
1168  else
1169  {
1170  SoSeparator* sep = layer.addedRobotVisualizations[d.name];
1171 
1172  for (int i = 0; i < sep->getNumChildren(); i++)
1173  {
1174  SoSeparator* nodeSep = dynamic_cast<SoSeparator*>(sep->getChild(i));
1175  SoMaterial* m = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
1176 
1177  if (!m)
1178  {
1179  ARMARX_ERROR << "Internal robot layer error2";
1180  return;
1181  }
1182 
1183  if (d.color.isNone())
1184  {
1185  m->setOverride(false);
1186  }
1187  else
1188  {
1189  if (d.color.r < 0 || d.color.g < 0 || d.color.b < 0)
1190  {
1191  }
1192 
1193  m->diffuseColor = SbColor(d.color.r, d.color.g, d.color.b);
1194  m->ambientColor = SbColor(0, 0, 0);
1195  m->transparency = std::max(0.0f, d.color.transparency);
1196  m->setOverride(true);
1197  }
1198  }
1199  }
1200  }
1201 
1202  if (d.updateNodeColor)
1203  {
1204  if (layer.addedRobotVisualizations.find(d.name) == layer.addedRobotVisualizations.end())
1205  {
1206  ARMARX_WARNING << deactivateSpam() << "Internal robot visu error";
1207  }
1208  else
1209  {
1210  SoSeparator* sep = layer.addedRobotVisualizations[d.name];
1211 
1212  if (!sep || sep->getNumChildren() < 2)
1213  {
1214  ARMARX_ERROR << "Internal robot layer error1";
1215  return;
1216  }
1217 
1218  std::string entryName = simox::alg::replace_all(
1219  std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
1220  VirtualRobot::RobotPtr robot = activeRobots[entryName];
1221 
1222  std::vector<std::string> nodeNameList;
1223 
1224  for (size_t i = 0; i < robot->getRobotNodes().size(); ++i)
1225  {
1226  nodeNameList.push_back(robot->getRobotNodes()[i]->getName());
1227  }
1228 
1229  for (std::map<std::string,
1230  VirtualRobot::VisualizationFactory::Color>::const_iterator it =
1231  d.nodeColors.begin();
1232  it != d.nodeColors.end();
1233  it++)
1234  {
1235 
1236  std::string nodeName = it->first;
1237  VirtualRobot::VisualizationFactory::Color nodeColor = it->second;
1238 
1239  std::vector<std::string>::iterator strit;
1240  strit = std::find(nodeNameList.begin(), nodeNameList.end(), nodeName);
1241 
1242  if (strit == nodeNameList.end())
1243  {
1244  ARMARX_WARNING << "Cannot find correct robot node name " + nodeName +
1245  " for DebugDrawer node color update !!!";
1246  continue;
1247  }
1248 
1249  size_t ind = strit - nodeNameList.begin();
1250  SoSeparator* nodeSep = dynamic_cast<SoSeparator*>(sep->getChild(ind));
1251 
1252  if (!nodeSep)
1253  {
1254  ARMARX_ERROR << "Internal robot layer error3";
1255  return;
1256  }
1257 
1258  SoMaterial* nodeMat = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
1259 
1260  if (!nodeMat)
1261  {
1262  ARMARX_ERROR << "Internal robot layer error4";
1263  return;
1264  }
1265 
1266  if (nodeColor.isNone())
1267  {
1268  nodeMat->setOverride(false);
1269  }
1270  else
1271  {
1272  if (nodeColor.r < 0 || nodeColor.g < 0 || nodeColor.b < 0)
1273  {
1274  ARMARX_ERROR << "Internal robot layer node color error";
1275  return;
1276  }
1277 
1278  nodeMat->diffuseColor = SbColor(nodeColor.r, nodeColor.g, nodeColor.b);
1279  nodeMat->ambientColor = SbColor(0, 0, 0);
1280  nodeMat->transparency = std::max(0.0f, nodeColor.transparency);
1281  nodeMat->setOverride(true);
1282  }
1283  }
1284  }
1285  }
1286  }
1287 
1288  VirtualRobot::RobotPtr
1289  DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
1290  {
1291  auto l = getScopedVisuLock();
1292 
1293  auto& layer = requestLayer(d.layerName);
1294  std::string entryName = simox::alg::replace_all(
1295  std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
1296 
1297  ARMARX_DEBUG << "Requesting robot " << entryName;
1298 
1299  if (activeRobots.find(entryName) != activeRobots.end())
1300  {
1301  ARMARX_DEBUG << "Found robot " << entryName << ":"
1302  << activeRobots[entryName]->getName();
1303  return activeRobots[entryName];
1304  }
1305 
1306  VirtualRobot::RobotPtr result;
1307 
1308  if (d.robotFile.empty())
1309  {
1310  ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName
1311  << ", with name " << d.name;
1312  return result;
1313  }
1314 
1315  if (!d.armarxProject.empty())
1316  {
1317  ARMARX_INFO << "Adding to datapaths of " << d.armarxProject;
1318  armarx::CMakePackageFinder finder(d.armarxProject);
1319 
1320  if (!finder.packageFound())
1321  {
1322  ARMARX_WARNING << "ArmarX Package " << d.armarxProject << " has not been found!";
1323  }
1324  else
1325  {
1326  ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
1327  armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
1328  }
1329  }
1330 
1331  // load robot
1332  std::string filename = d.robotFile;
1333  ArmarXDataPath::getAbsolutePath(filename, filename);
1334 
1335  VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
1336  VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
1337 
1338  if (d.drawStyle == DrawStyle::CollisionModel)
1339  {
1340  visuType = VirtualRobot::SceneObject::Collision;
1341  loadMode = VirtualRobot::RobotIO::eCollisionModel;
1342  }
1343 
1344  ARMARX_INFO << "Loading robot from " << filename;
1345 
1346  try
1347  {
1348  if (loadedRobots.find(filename) == loadedRobots.end())
1349  {
1350  loadedRobots[filename] = RobotIO::loadRobot(filename, loadMode);
1351  }
1352 
1353  result = loadedRobots[filename]->clone(d.name);
1354  }
1355  catch (...)
1356  {
1357  }
1358 
1359  if (!result)
1360  {
1361  ARMARX_IMPORTANT << "Robot loading failed, file:" << filename;
1362  return result;
1363  }
1364 
1365  SoSeparator* sep = new SoSeparator;
1366  layer.mainNode->addChild(sep);
1367 
1368  for (size_t i = 0; i < result->getRobotNodes().size(); ++i)
1369  {
1370  VirtualRobot::RobotNodePtr robNode = result->getRobotNodes()[i];
1371  SoSeparator* nodeSep = new SoSeparator;
1372 
1373  SoMaterial* nodeMat = new SoMaterial;
1374  nodeMat->setOverride(false);
1375  nodeSep->addChild(nodeMat);
1376 
1377  CoinVisualizationPtr robNodeVisu =
1378  robNode->getVisualization<CoinVisualization>(visuType);
1379 
1380  if (robNodeVisu)
1381  {
1382  SoNode* sepRobNode = robNodeVisu->getCoinVisualization();
1383 
1384  if (sepRobNode)
1385  {
1386  nodeSep->addChild(sepRobNode);
1387  }
1388  }
1389 
1390  sep->addChild(nodeSep);
1391  }
1392 
1393  activeRobots[entryName] = result;
1394  ARMARX_DEBUG << "setting robot to activeRobots, entryName:" << entryName
1395  << ", activeRobots.size():" << activeRobots.size();
1396  layer.addedRobotVisualizations[d.name] = sep;
1397  ARMARX_DEBUG << "adding sep to layer.addedRobotVisualizations, d.name:" << d.name
1398  << ", layer:" << d.layerName << ", layer.addedRobotVisualizations.size():"
1399  << layer.addedRobotVisualizations.size();
1400 
1401  return result;
1402  }
1403 
1404  void
1405  DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
1406  {
1407  auto l = getScopedVisuLock();
1408 
1409  if (!hasLayer(layerName))
1410  {
1411  return;
1412  }
1413 
1414  auto& layer = layers.at(layerName);
1415 
1416  if (layer.addedLineVisualizations.find(name) == layer.addedLineVisualizations.end())
1417  {
1418  return;
1419  }
1420 
1421  layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
1422  layer.addedLineVisualizations.erase(name);
1423  }
1424 
1425  void
1426  DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
1427  {
1428  auto l = getScopedVisuLock();
1429 
1430  if (!hasLayer(layerName))
1431  {
1432  return;
1433  }
1434 
1435  auto& layer = layers.at(layerName);
1436 
1437  if (layer.addedLineSetVisualizations.find(name) == layer.addedLineSetVisualizations.end())
1438  {
1439  return;
1440  }
1441 
1442  layer.mainNode->removeChild(layer.addedLineSetVisualizations[name]);
1443  layer.addedLineSetVisualizations.erase(name);
1444  }
1445 
1446  void
1447  DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
1448  {
1449  auto l = getScopedVisuLock();
1450 
1451  if (!hasLayer(layerName))
1452  {
1453  return;
1454  }
1455 
1456  auto& layer = layers.at(layerName);
1457 
1458  if (layer.addedBoxVisualizations.find(name) == layer.addedBoxVisualizations.end())
1459  {
1460  return;
1461  }
1462 
1463  layer.mainNode->removeChild(layer.addedBoxVisualizations[name]);
1464  layer.addedBoxVisualizations.erase(name);
1465  }
1466 
1467  void
1468  DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
1469  {
1470  auto l = getScopedVisuLock();
1471 
1472  if (!hasLayer(layerName))
1473  {
1474  return;
1475  }
1476 
1477  auto& layer = layers.at(layerName);
1478 
1479  if (layer.addedTextVisualizations.find(name) == layer.addedTextVisualizations.end())
1480  {
1481  return;
1482  }
1483 
1484  layer.mainNode->removeChild(layer.addedTextVisualizations[name]);
1485  layer.addedTextVisualizations.erase(name);
1486  }
1487 
1488  void
1489  DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
1490  {
1491  auto l = getScopedVisuLock();
1492 
1493  if (!hasLayer(layerName))
1494  {
1495  return;
1496  }
1497 
1498  auto& layer = layers.at(layerName);
1499 
1500  if (layer.addedSphereVisualizations.find(name) == layer.addedSphereVisualizations.end())
1501  {
1502  return;
1503  }
1504 
1505  layer.mainNode->removeChild(layer.addedSphereVisualizations[name]);
1506  layer.addedSphereVisualizations.erase(name);
1507  }
1508 
1509  void
1510  DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
1511  {
1512  auto l = getScopedVisuLock();
1513 
1514  if (!hasLayer(layerName))
1515  {
1516  return;
1517  }
1518 
1519  auto& layer = layers.at(layerName);
1520 
1521  if (layer.addedCylinderVisualizations.find(name) == layer.addedCylinderVisualizations.end())
1522  {
1523  return;
1524  }
1525 
1526  layer.mainNode->removeChild(layer.addedCylinderVisualizations[name]);
1527  layer.addedCylinderVisualizations.erase(name);
1528  }
1529 
1530  void
1531  DebugDrawerComponent::removeCircle(const std::string& layerName, const std::string& name)
1532  {
1533  auto l = getScopedVisuLock();
1534 
1535  if (!hasLayer(layerName))
1536  {
1537  return;
1538  }
1539 
1540  auto& layer = layers.at(layerName);
1541 
1542  if (layer.addedCircleVisualizations.find(name) == layer.addedCircleVisualizations.end())
1543  {
1544  return;
1545  }
1546 
1547  layer.mainNode->removeChild(layer.addedCircleVisualizations[name]);
1548  layer.addedCircleVisualizations.erase(name);
1549  }
1550 
1551  void
1552  DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
1553  {
1554  auto l = getScopedVisuLock();
1555 
1556  if (!hasLayer(layerName))
1557  {
1558  return;
1559  }
1560 
1561  auto& layer = layers.at(layerName);
1562 
1563  if (layer.addedPointCloudVisualizations.find(name) ==
1564  layer.addedPointCloudVisualizations.end())
1565  {
1566  return;
1567  }
1568 
1569  layer.mainNode->removeChild(layer.addedPointCloudVisualizations[name]);
1570  layer.addedPointCloudVisualizations.erase(name);
1571  }
1572 
1573  void
1574  DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
1575  {
1576  auto l = getScopedVisuLock();
1577 
1578  if (!hasLayer(layerName))
1579  {
1580  return;
1581  }
1582 
1583  auto& layer = layers.at(layerName);
1584 
1585  if (layer.addedPolygonVisualizations.find(name) == layer.addedPolygonVisualizations.end())
1586  {
1587  return;
1588  }
1589 
1590  layer.mainNode->removeChild(layer.addedPolygonVisualizations[name]);
1591  layer.addedPolygonVisualizations.erase(name);
1592  }
1593 
1594  void
1595  DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
1596  {
1597  auto l = getScopedVisuLock();
1598 
1599  if (!hasLayer(layerName))
1600  {
1601  return;
1602  }
1603 
1604  auto& layer = layers.at(layerName);
1605 
1606  if (layer.addedTriMeshVisualizations.find(name) == layer.addedTriMeshVisualizations.end())
1607  {
1608  return;
1609  }
1610 
1611  layer.mainNode->removeChild(layer.addedTriMeshVisualizations[name]);
1612  layer.addedTriMeshVisualizations.erase(name);
1613  }
1614 
1615  void
1616  DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
1617  {
1618  auto l = getScopedVisuLock();
1619 
1620  if (!hasLayer(layerName))
1621  {
1622  return;
1623  }
1624 
1625  auto& layer = layers.at(layerName);
1626 
1627  if (layer.addedArrowVisualizations.find(name) == layer.addedArrowVisualizations.end())
1628  {
1629  return;
1630  }
1631 
1632  layer.mainNode->removeChild(layer.addedArrowVisualizations[name]);
1633  layer.addedArrowVisualizations.erase(name);
1634  }
1635 
1636  void
1637  DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
1638  {
1639  auto l = getScopedVisuLock();
1640 
1641  // process active robots
1642  std::string entryName =
1643  simox::alg::replace_all(std::string("__" + layerName + "__" + name + "__"), " ", "_");
1644  ARMARX_DEBUG << "Removing robot " << entryName;
1645 
1646  if (activeRobots.find(entryName) != activeRobots.end())
1647  {
1648  ARMARX_DEBUG << "Found robot to remove " << entryName;
1649  activeRobots.erase(entryName);
1650  ARMARX_DEBUG << "after Found robot to remove, activeRobots.size() = "
1651  << activeRobots.size();
1652  }
1653 
1654  // process visualizations
1655  if (!hasLayer(layerName))
1656  {
1657  ARMARX_DEBUG << "Layer not found " << layerName;
1658  return;
1659  }
1660 
1661  auto& layer = layers.at(layerName);
1662 
1663  if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
1664  {
1665  ARMARX_INFO << "Could not find robot with name " << name;
1666  return;
1667  }
1668 
1669  ARMARX_DEBUG << "Removing visualization for " << entryName;
1670 
1671  ARMARX_DEBUG << "removing sep from layer.addedRobotVisualizations, d.name:" << name
1672  << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():"
1673  << layer.addedRobotVisualizations.size();
1674 
1675  if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
1676  {
1677  ARMARX_WARNING << "separator not found...";
1678  return;
1679  }
1680 
1681  ARMARX_DEBUG << "removing from layer.mainNode, layer.mainNode->getNumChildren()="
1682  << layer.mainNode->getNumChildren();
1683 
1684  if (layer.mainNode->findChild(layer.addedRobotVisualizations[name]) < 0)
1685  {
1686  ARMARX_WARNING << "separator with wrong index...";
1687  return;
1688  }
1689 
1690  layer.mainNode->removeChild(layer.addedRobotVisualizations[name]);
1691  layer.addedRobotVisualizations.erase(name);
1692  ARMARX_DEBUG << "after removing from layer.mainNode, layer.mainNode->getNumChildren()="
1693  << layer.mainNode->getNumChildren();
1694  ARMARX_DEBUG << "after removing sep from layer.addedRobotVisualizations, d.name:" << name
1695  << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():"
1696  << layer.addedRobotVisualizations.size();
1697  }
1698 
1699  void
1700  DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
1701  {
1702  auto l = getScopedVisuLock();
1703 
1704  if (!hasLayer(layerName))
1705  {
1706  return;
1707  }
1708 
1709  auto& layer = layers.at(layerName);
1710 
1711  if (layer.addedCoordVisualizations.find(name) == layer.addedCoordVisualizations.end())
1712  {
1713  return;
1714  }
1715 
1716  layer.mainNode->removeChild(layer.addedCoordVisualizations[name]);
1717  layer.addedCoordVisualizations.erase(name);
1718  }
1719 
1720  void
1721  DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
1722  {
1723  auto l = getScopedVisuLock();
1724 
1725  if (!hasLayer(layerName))
1726  {
1727  return;
1728  }
1729 
1730  auto& layer = layers.at(layerName);
1731  layer.visible = visible;
1732 
1733  if (visible)
1734  {
1735  if (layerMainNode->findChild(layer.mainNode) < 0)
1736  {
1737  layerMainNode->addChild(layer.mainNode);
1738  }
1739  }
1740  else
1741  {
1742  if (layerMainNode->findChild(layer.mainNode) >= 0)
1743  {
1744  layerMainNode->removeChild(layer.mainNode);
1745  }
1746  }
1747  }
1748 
1749  void
1750  DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
1751  {
1752  auto l = getScopedVisuLock();
1753 
1754  if (selectionNode->findChild(layerMainNode) >= 0)
1755  {
1756  selectionNode->removeChild(layerMainNode);
1757  }
1758  }
1759 
1760  void
1761  DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
1762  {
1763  auto l = getScopedVisuLock();
1764 
1765  if (selectionNode->findChild(layerMainNode) < 0)
1766  {
1767  selectionNode->addChild(layerMainNode);
1768  }
1769  }
1770 
1771  void
1772  DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName,
1773  const std::string& poseName,
1774  const ::armarx::PoseBasePtr& globalPose,
1775  const ::Ice::Float scale,
1776  const ::Ice::Current&)
1777  {
1778  ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
1779  Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
1780  {
1781  auto l = getScopedAccumulatedDataLock();
1782  std::string entryName = simox::alg::replace_all(
1783  std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
1784  CoordData& d = accumulatedUpdateData.coord[entryName];
1785  d.globalPose = gp;
1786  d.layerName = layerName;
1787  d.name = poseName;
1788  d.scale = scale;
1789  d.active = true;
1790  }
1791  }
1792 
1793  void
1794  DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName,
1795  const ::armarx::PoseBasePtr& globalPose,
1796  const ::Ice::Float scale,
1797  const ::Ice::Current&)
1798  {
1799  setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, scale);
1800  }
1801 
1802  void
1803  DebugDrawerComponent::setPoseVisu(const std::string& layerName,
1804  const std::string& poseName,
1805  const PoseBasePtr& globalPose,
1806  const Ice::Current&)
1807  {
1808  setScaledPoseVisu(layerName, poseName, globalPose, 1.f);
1809  }
1810 
1811  void
1812  DebugDrawerComponent::setPoseDebugLayerVisu(const std::string& poseName,
1813  const PoseBasePtr& globalPose,
1814  const Ice::Current&)
1815  {
1816  setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, 1.f);
1817  }
1818 
1819  void
1820  DebugDrawerComponent::removePoseVisu(const std::string& layerName,
1821  const std::string& poseName,
1822  const Ice::Current&)
1823  {
1824  {
1825  auto l = getScopedAccumulatedDataLock();
1826  std::string entryName = simox::alg::replace_all(
1827  std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
1828  CoordData& d = accumulatedUpdateData.coord[entryName];
1829  d.layerName = layerName;
1830  d.name = poseName;
1831  d.active = false;
1832  }
1833  }
1834 
1835  void
1836  DebugDrawerComponent::removePoseDebugLayerVisu(const std::string& poseName, const Ice::Current&)
1837  {
1838  removePoseVisu(DEBUG_LAYER_NAME, poseName);
1839  }
1840 
1841  void
1842  DebugDrawerComponent::setLineVisu(const std::string& layerName,
1843  const std::string& lineName,
1844  const Vector3BasePtr& globalPosition1,
1845  const Vector3BasePtr& globalPosition2,
1846  float lineWidth,
1847  const DrawColor& color,
1848  const Ice::Current&)
1849  {
1850  Eigen::Vector3f p1 = Vector3Ptr::dynamicCast(globalPosition1)->toEigen();
1851  Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
1852  VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
1853  {
1854  auto l = getScopedAccumulatedDataLock();
1855  std::string entryName = simox::alg::replace_all(
1856  std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
1857  LineData& d = accumulatedUpdateData.line[entryName];
1858  d.p1 = p1;
1859  d.p2 = p2;
1860  d.layerName = layerName;
1861  d.name = lineName;
1862  d.color = c;
1863  d.scale = lineWidth;
1864  d.active = true;
1865  }
1866  }
1867 
1868  void
1869  DebugDrawerComponent::setLineDebugLayerVisu(const std::string& lineName,
1870  const Vector3BasePtr& globalPosition1,
1871  const Vector3BasePtr& globalPosition2,
1872  float lineWidth,
1873  const DrawColor& color,
1874  const Ice::Current&)
1875  {
1876  setLineVisu(DEBUG_LAYER_NAME, lineName, globalPosition1, globalPosition2, lineWidth, color);
1877  }
1878 
1879  void
1880  DebugDrawerComponent::removeLineVisu(const std::string& layerName,
1881  const std::string& lineName,
1882  const Ice::Current&)
1883  {
1884  {
1885  auto l = getScopedAccumulatedDataLock();
1886  std::string entryName = simox::alg::replace_all(
1887  std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
1888  LineData& d = accumulatedUpdateData.line[entryName];
1889  d.layerName = layerName;
1890  d.name = lineName;
1891  d.active = false;
1892  }
1893  }
1894 
1895  void
1896  DebugDrawerComponent::removeLineDebugLayerVisu(const std::string& lineName, const Ice::Current&)
1897  {
1898  removeLineVisu(DEBUG_LAYER_NAME, lineName);
1899  }
1900 
1901  void
1902  DebugDrawerComponent::setLineSetVisu(const std::string& layerName,
1903  const std::string& lineSetName,
1904  const DebugDrawerLineSet& lineSet,
1905  const Ice::Current&)
1906  {
1907  ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
1908  {
1909  auto l = getScopedAccumulatedDataLock();
1910  std::string entryName = simox::alg::replace_all(
1911  std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
1912  LineSetData& d = accumulatedUpdateData.lineSet[entryName];
1913  d.lineSet = lineSet;
1914  d.layerName = layerName;
1915  d.name = lineSetName;
1916  d.active = true;
1917  }
1918  }
1919 
1920  void
1921  DebugDrawerComponent::setLineSetDebugLayerVisu(const std::string& lineSetName,
1922  const DebugDrawerLineSet& lineSet,
1923  const Ice::Current&)
1924  {
1925  setLineSetVisu(DEBUG_LAYER_NAME, lineSetName, lineSet);
1926  }
1927 
1928  void
1929  DebugDrawerComponent::removeLineSetVisu(const std::string& layerName,
1930  const std::string& lineSetName,
1931  const Ice::Current&)
1932  {
1933  {
1934  auto l = getScopedAccumulatedDataLock();
1935  std::string entryName = simox::alg::replace_all(
1936  std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
1937  LineSetData& d = accumulatedUpdateData.lineSet[entryName];
1938  d.layerName = layerName;
1939  d.name = lineSetName;
1940  d.active = false;
1941  }
1942  }
1943 
1944  void
1945  DebugDrawerComponent::removeLineSetDebugLayerVisu(const std::string& lineSetName,
1946  const Ice::Current&)
1947  {
1948  removeLineSetVisu(DEBUG_LAYER_NAME, lineSetName);
1949  }
1950 
1951  void
1952  DebugDrawerComponent::setBoxVisu(const std::string& layerName,
1953  const std::string& boxName,
1954  const PoseBasePtr& globalPose,
1955  const Vector3BasePtr& dimensions,
1956  const DrawColor& color,
1957  const Ice::Current&)
1958  {
1959  Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
1960  VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
1961  {
1962  auto l = getScopedAccumulatedDataLock();
1963  std::string entryName = simox::alg::replace_all(
1964  std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
1965  BoxData& d = accumulatedUpdateData.box[entryName];
1966  d.width = dimensions->x;
1967  d.height = dimensions->y;
1968  d.depth = dimensions->z;
1969  d.layerName = layerName;
1970  d.name = boxName;
1971  d.color = c;
1972  d.globalPose = gp;
1973  d.active = true;
1974  }
1975  }
1976 
1977  void
1978  DebugDrawerComponent::setBoxDebugLayerVisu(const std::string& boxName,
1979  const PoseBasePtr& globalPose,
1980  const Vector3BasePtr& dimensions,
1981  const DrawColor& color,
1982  const Ice::Current&)
1983  {
1984  setBoxVisu(DEBUG_LAYER_NAME, boxName, globalPose, dimensions, color);
1985  }
1986 
1987  void
1988  DebugDrawerComponent::removeBoxVisu(const std::string& layerName,
1989  const std::string& boxName,
1990  const Ice::Current&)
1991  {
1992  {
1993  auto l = getScopedAccumulatedDataLock();
1994  std::string entryName = simox::alg::replace_all(
1995  std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
1996  BoxData& d = accumulatedUpdateData.box[entryName];
1997  d.layerName = layerName;
1998  d.name = boxName;
1999  d.active = false;
2000  }
2001  }
2002 
2003  void
2004  DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string& boxName, const Ice::Current&)
2005  {
2006  removeBoxVisu(DEBUG_LAYER_NAME, boxName);
2007  }
2008 
2009  void
2010  DebugDrawerComponent::setTextVisu(const std::string& layerName,
2011  const std::string& textName,
2012  const std::string& text,
2013  const Vector3BasePtr& globalPosition,
2014  const DrawColor& color,
2015  int size,
2016  const Ice::Current&)
2017  {
2018  ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
2019  {
2020  auto l = getScopedAccumulatedDataLock();
2021  std::string entryName = simox::alg::replace_all(
2022  std::string("__" + layerName + "__" + textName + "__"), " ", "_");
2023  TextData& d = accumulatedUpdateData.text[entryName];
2024  d.text = text;
2025  d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
2026  d.layerName = layerName;
2027  d.name = textName;
2028  d.color =
2029  VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
2030  d.size = size;
2031  d.active = true;
2032  }
2033  }
2034 
2035  void
2036  DebugDrawerComponent::setTextDebugLayerVisu(const std::string& textName,
2037  const std::string& text,
2038  const Vector3BasePtr& globalPosition,
2039  const DrawColor& color,
2040  int size,
2041  const Ice::Current&)
2042  {
2043  setTextVisu(DEBUG_LAYER_NAME, textName, text, globalPosition, color, size);
2044  }
2045 
2046  void
2047  DebugDrawerComponent::removeTextVisu(const std::string& layerName,
2048  const std::string& textName,
2049  const Ice::Current&)
2050  {
2051  {
2052  auto l = getScopedAccumulatedDataLock();
2053  std::string entryName = simox::alg::replace_all(
2054  std::string("__" + layerName + "__" + textName + "__"), " ", "_");
2055  TextData& d = accumulatedUpdateData.text[entryName];
2056  d.layerName = layerName;
2057  d.name = textName;
2058  d.active = false;
2059  }
2060  }
2061 
2062  void
2063  DebugDrawerComponent::removeTextDebugLayerVisu(const std::string& textName, const Ice::Current&)
2064  {
2065  removeTextVisu(DEBUG_LAYER_NAME, textName);
2066  }
2067 
2068  void
2069  DebugDrawerComponent::setSphereVisu(const std::string& layerName,
2070  const std::string& sphereName,
2071  const Vector3BasePtr& globalPosition,
2072  const DrawColor& color,
2073  float radius,
2074  const Ice::Current&)
2075  {
2076  ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
2077  {
2078  auto l = getScopedAccumulatedDataLock();
2079  std::string entryName = simox::alg::replace_all(
2080  std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
2081  SphereData& d = accumulatedUpdateData.sphere[entryName];
2082  d.radius = radius;
2083  d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
2084  d.layerName = layerName;
2085  d.name = sphereName;
2086  d.color =
2087  VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
2088  d.active = true;
2089  }
2090  }
2091 
2092  void
2093  DebugDrawerComponent::setSphereDebugLayerVisu(const std::string& sphereName,
2094  const Vector3BasePtr& globalPosition,
2095  const DrawColor& color,
2096  float radius,
2097  const Ice::Current&)
2098  {
2099  setSphereVisu(DEBUG_LAYER_NAME, sphereName, globalPosition, color, radius);
2100  }
2101 
2102  void
2103  DebugDrawerComponent::removeSphereVisu(const std::string& layerName,
2104  const std::string& sphereName,
2105  const Ice::Current&)
2106  {
2107  {
2108  auto l = getScopedAccumulatedDataLock();
2109  std::string entryName = simox::alg::replace_all(
2110  std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
2111  SphereData& d = accumulatedUpdateData.sphere[entryName];
2112  d.layerName = layerName;
2113  d.name = sphereName;
2114  d.active = false;
2115  }
2116  }
2117 
2118  void
2119  DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string& sphereName,
2120  const Ice::Current&)
2121  {
2122  removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
2123  }
2124 
2125  void
2126  DebugDrawerComponent::setCylinderVisu(const std::string& layerName,
2127  const std::string& cylinderName,
2128  const Vector3BasePtr& globalPosition,
2129  const Vector3BasePtr& direction,
2130  float length,
2131  float radius,
2132  const DrawColor& color,
2133  const Ice::Current&)
2134  {
2135  ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
2136  {
2137  auto l = getScopedAccumulatedDataLock();
2138  std::string entryName = simox::alg::replace_all(
2139  std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
2140  CylinderData& d = accumulatedUpdateData.cylinder[entryName];
2141  d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
2142  d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
2143  d.length = length;
2144  d.radius = radius;
2145  d.layerName = layerName;
2146  d.name = cylinderName;
2147  d.color =
2148  VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
2149  d.active = true;
2150  }
2151  }
2152 
2153  void
2154  DebugDrawerComponent::setCylinderDebugLayerVisu(const std::string& cylinderName,
2155  const Vector3BasePtr& globalPosition,
2156  const Vector3BasePtr& direction,
2157  float length,
2158  float radius,
2159  const DrawColor& color,
2160  const Ice::Current&)
2161  {
2162  setCylinderVisu(
2163  DEBUG_LAYER_NAME, cylinderName, globalPosition, direction, length, radius, color);
2164  }
2165 
2166  void
2167  DebugDrawerComponent::removeCylinderVisu(const std::string& layerName,
2168  const std::string& cylinderName,
2169  const Ice::Current&)
2170  {
2171  {
2172  auto l = getScopedAccumulatedDataLock();
2173  std::string entryName = simox::alg::replace_all(
2174  std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
2175  CylinderData& d = accumulatedUpdateData.cylinder[entryName];
2176  d.layerName = layerName;
2177  d.name = cylinderName;
2178  d.active = false;
2179  }
2180  }
2181 
2182  void
2183  DebugDrawerComponent::removeCylinderDebugLayerVisu(const std::string& cylinderName,
2184  const Ice::Current&)
2185  {
2186  removeCylinderVisu(DEBUG_LAYER_NAME, cylinderName);
2187  }
2188 
2189  void
2190  DebugDrawerComponent::setPointCloudVisu(const std::string& layerName,
2191  const std::string& pointCloudName,
2192  const DebugDrawerPointCloud& pointCloud,
2193  const Ice::Current&)
2194  {
2195  ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
2196  {
2197  auto l = getScopedAccumulatedDataLock();
2198  std::string entryName = simox::alg::replace_all(
2199  std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
2200  PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
2201  d.pointCloud = pointCloud;
2202  d.layerName = layerName;
2203  d.name = pointCloudName;
2204  d.active = true;
2205  }
2206  }
2207 
2208  void
2209  DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string& pointCloudName,
2210  const DebugDrawerPointCloud& pointCloud,
2211  const Ice::Current&)
2212  {
2213  setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
2214  }
2215 
2216  void
2217  DebugDrawerComponent::removePointCloudVisu(const std::string& layerName,
2218  const std::string& pointCloudName,
2219  const Ice::Current&)
2220  {
2221  {
2222  auto l = getScopedAccumulatedDataLock();
2223  std::string entryName = simox::alg::replace_all(
2224  std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
2225  PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
2226  d.layerName = layerName;
2227  d.name = pointCloudName;
2228  d.active = false;
2229  }
2230  }
2231 
2232  void
2233  DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string& pointCloudName,
2234  const Ice::Current&)
2235  {
2236  removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
2237  }
2238 
2239  void
2240  DebugDrawerComponent::setPolygonVisu(const std::string& layerName,
2241  const std::string& polygonName,
2242  const std::vector<Vector3BasePtr>& polygonPoints,
2243  const DrawColor& colorInner,
2244  const DrawColor& colorBorder,
2245  float lineWidth,
2246  const Ice::Current&)
2247  {
2248  {
2249  auto l = getScopedAccumulatedDataLock();
2250  std::string entryName = simox::alg::replace_all(
2251  std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
2252  PolygonData& d = accumulatedUpdateData.polygons[entryName];
2253 
2254  std::vector<Eigen::Vector3f> points;
2255 
2256  for (size_t i = 0; i < polygonPoints.size(); i++)
2257  {
2258  Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();
2259  ;
2260  points.push_back(p);
2261  }
2262 
2263  d.points = points;
2264  d.colorInner = VirtualRobot::VisualizationFactory::Color(
2265  colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);
2266  ;
2267  d.colorBorder = VirtualRobot::VisualizationFactory::Color(
2268  colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);
2269  ;
2270 
2271  d.lineWidth = lineWidth;
2272  d.layerName = layerName;
2273  d.name = polygonName;
2274  d.active = true;
2275  }
2276  }
2277 
2278  void
2279  DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string& polygonName,
2280  const std::vector<Vector3BasePtr>& polygonPoints,
2281  const DrawColor& colorInner,
2282  const DrawColor& colorBorder,
2283  float lineWidth,
2284  const Ice::Current&)
2285  {
2286  setPolygonVisu(
2287  DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
2288  }
2289 
2290  void
2291  DebugDrawerComponent::removePolygonVisu(const std::string& layerName,
2292  const std::string& polygonName,
2293  const Ice::Current&)
2294  {
2295  {
2296  auto l = getScopedAccumulatedDataLock();
2297  std::string entryName = simox::alg::replace_all(
2298  std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
2299  PolygonData& d = accumulatedUpdateData.polygons[entryName];
2300  d.layerName = layerName;
2301  d.name = polygonName;
2302  d.active = false;
2303  }
2304  }
2305 
2306  void
2307  DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string& polygonName,
2308  const Ice::Current&)
2309  {
2310  removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
2311  }
2312 
2313  void
2314  DebugDrawerComponent::setTriMeshVisu(const std::string& layerName,
2315  const std::string& triMeshName,
2316  const DebugDrawerTriMesh& triMesh,
2317  const Ice::Current&)
2318  {
2319  {
2320  auto l = getScopedAccumulatedDataLock();
2321  std::string entryName = simox::alg::replace_all(
2322  std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
2323  TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
2324  d.triMesh = triMesh;
2325  d.layerName = layerName;
2326  d.name = triMeshName;
2327  d.active = true;
2328  }
2329  }
2330 
2331  void
2332  DebugDrawerComponent::setTriMeshDebugLayerVisu(const std::string& triMeshName,
2333  const DebugDrawerTriMesh& triMesh,
2334  const Ice::Current&)
2335  {
2336  setTriMeshVisu(DEBUG_LAYER_NAME, triMeshName, triMesh);
2337  }
2338 
2339  void
2340  DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName,
2341  const std::string& triMeshName,
2342  const Ice::Current&)
2343  {
2344  {
2345  auto l = getScopedAccumulatedDataLock();
2346  std::string entryName = simox::alg::replace_all(
2347  std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
2348  TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
2349  d.layerName = layerName;
2350  d.name = triMeshName;
2351  d.active = false;
2352  }
2353  }
2354 
2355  void
2356  DebugDrawerComponent::removeTriMeshDebugLayerVisu(const std::string& triMeshName,
2357  const Ice::Current&)
2358  {
2359  removeTriMeshVisu(DEBUG_LAYER_NAME, triMeshName);
2360  }
2361 
2362  void
2363  DebugDrawerComponent::setArrowVisu(const std::string& layerName,
2364  const std::string& arrowName,
2365  const Vector3BasePtr& position,
2366  const Vector3BasePtr& direction,
2367  const DrawColor& color,
2368  float length,
2369  float width,
2370  const Ice::Current&)
2371  {
2372  {
2373  auto l = getScopedAccumulatedDataLock();
2374  std::string entryName = simox::alg::replace_all(
2375  std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
2376  ArrowData& d = accumulatedUpdateData.arrows[entryName];
2377  d.position = Vector3Ptr::dynamicCast(position)->toEigen();
2378  d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
2379  d.color =
2380  VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
2381  ;
2382  d.length = length;
2383  d.width = width;
2384  d.layerName = layerName;
2385  d.name = arrowName;
2386  d.active = true;
2387  }
2388  }
2389 
2390  void
2391  DebugDrawerComponent::setArrowDebugLayerVisu(const std::string& arrowName,
2392  const Vector3BasePtr& position,
2393  const Vector3BasePtr& direction,
2394  const DrawColor& color,
2395  float length,
2396  float width,
2397  const Ice::Current&)
2398  {
2399  setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
2400  }
2401 
2402  void
2403  DebugDrawerComponent::removeArrowVisu(const std::string& layerName,
2404  const std::string& arrowName,
2405  const Ice::Current&)
2406  {
2407  {
2408  auto l = getScopedAccumulatedDataLock();
2409  std::string entryName = simox::alg::replace_all(
2410  std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
2411  ArrowData& d = accumulatedUpdateData.arrows[entryName];
2412  d.layerName = layerName;
2413  d.name = arrowName;
2414  d.active = false;
2415  }
2416  }
2417 
2418  void
2419  DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string& arrowName,
2420  const Ice::Current&)
2421  {
2422  removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
2423  }
2424 
2425  void
2426  DebugDrawerComponent::setRobotVisu(const std::string& layerName,
2427  const std::string& robotName,
2428  const std::string& robotFile,
2429  const std::string& armarxProject,
2430  DrawStyle drawStyle,
2431  const Ice::Current&)
2432  {
2433  auto l = getScopedAccumulatedDataLock();
2434  std::string entryName = simox::alg::replace_all(
2435  std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
2436  ARMARX_DEBUG << "setting robot visualization for " << entryName;
2437  RobotData& d = accumulatedUpdateData.robots[entryName];
2438 
2439  d.robotFile = robotFile;
2440  d.armarxProject = armarxProject;
2441  d.drawStyle = drawStyle;
2442 
2443  d.layerName = layerName;
2444  d.name = robotName;
2445  d.active = true;
2446  }
2447 
2448  void
2449  DebugDrawerComponent::updateRobotPose(const std::string& layerName,
2450  const std::string& robotName,
2451  const PoseBasePtr& globalPose,
2452  const Ice::Current&)
2453  {
2454  auto l = getScopedAccumulatedDataLock();
2455  std::string entryName = simox::alg::replace_all(
2456  std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
2457  ARMARX_DEBUG << "updating robot pose for " << entryName;
2458  RobotData& d = accumulatedUpdateData.robots[entryName];
2459 
2460  // update data
2461  d.update = true;
2462  d.updatePose = true;
2463  d.globalPose = PosePtr::dynamicCast(globalPose)->toEigen();
2464 
2465  d.layerName = layerName;
2466  d.name = robotName;
2467  d.active = true;
2468  }
2469 
2470  void
2471  DebugDrawerComponent::updateRobotConfig(const std::string& layerName,
2472  const std::string& robotName,
2473  const std::map<std::string, float>& configuration,
2474  const Ice::Current&)
2475  {
2476  auto l = getScopedAccumulatedDataLock();
2477  std::string entryName = simox::alg::replace_all(
2478  std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
2479  ARMARX_DEBUG << "updating robot config for " << entryName;
2480  RobotData& d = accumulatedUpdateData.robots[entryName];
2481 
2482  // update data
2483  d.update = true;
2484  d.updateConfig = true;
2485  d.layerName = layerName;
2486  d.name = robotName;
2487 
2488  for (auto& it : configuration)
2489  {
2490  d.configuration[it.first] = it.second;
2491  }
2492 
2493  d.active = true;
2494  }
2495 
2496  void
2497  DebugDrawerComponent::updateRobotColor(const std::string& layerName,
2498  const std::string& robotName,
2499  const DrawColor& c,
2500  const Ice::Current&)
2501  {
2502  auto l = getScopedAccumulatedDataLock();
2503  std::string entryName = simox::alg::replace_all(
2504  std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
2505  ARMARX_DEBUG << "updating robot color for " << entryName;
2506  RobotData& d = accumulatedUpdateData.robots[entryName];
2507 
2508  // update data
2509  d.update = true;
2510  d.updateColor = true;
2511  d.layerName = layerName;
2512  d.name = robotName;
2513 
2514  if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
2515  {
2516  d.color = VirtualRobot::VisualizationFactory::Color::None();
2517  }
2518  else
2519  {
2520  d.color = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
2521  }
2522 
2523  d.active = true;
2524  }
2525 
2526  void
2527  DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName,
2528  const std::string& robotName,
2529  const std::string& robotNodeName,
2530  const DrawColor& c,
2531  const Ice::Current&)
2532  {
2533  auto l = getScopedAccumulatedDataLock();
2534  std::string entryName = simox::alg::replace_all(
2535  std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
2536  ARMARX_DEBUG << "updating robot color for " << entryName;
2537  RobotData& d = accumulatedUpdateData.robots[entryName];
2538 
2539  d.update = true;
2540  d.updateNodeColor = true;
2541  d.layerName = layerName;
2542  d.name = robotName;
2543 
2544  if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
2545  {
2546  d.nodeColors[robotNodeName] = VirtualRobot::VisualizationFactory::Color::None();
2547  }
2548  else
2549  {
2550  d.nodeColors[robotNodeName] =
2551  VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
2552  }
2553 
2554  d.active = true;
2555  }
2556 
2557  void
2558  DebugDrawerComponent::removeRobotVisu(const std::string& layerName,
2559  const std::string& robotName,
2560  const Ice::Current&)
2561  {
2562  auto l = getScopedAccumulatedDataLock();
2563  std::string entryName = simox::alg::replace_all(
2564  std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
2565  ARMARX_DEBUG << "removing robot visu for " << entryName;
2566  RobotData& d = accumulatedUpdateData.robots[entryName];
2567 
2568  d.layerName = layerName;
2569  d.name = robotName;
2570  d.active = false;
2571  }
2572 
2573  void
2574  DebugDrawerComponent::clearAll(const Ice::Current&)
2575  {
2576  for (auto& i : layers)
2577  {
2578  clearLayer(i.first);
2579  }
2580  }
2581 
2582  void
2583  DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
2584  {
2585  auto lockData = getScopedAccumulatedDataLock();
2586  //updates should only contain elements to add for each layer after the last clear for this layer was executed
2587  //this prevents a sequence add,clear,add to result in an empty layer
2588  //=>remove all objects pending for this layer
2589  removeAccumulatedData(layerName);
2590  accumulatedUpdateData.clearLayers.emplace(layerName);
2591  }
2592 
2593  void
2594  DebugDrawerComponent::clearLayerQt(const std::string& layerName)
2595  {
2596  auto lockData = getScopedAccumulatedDataLock();
2597  auto lockVisu = getScopedVisuLock();
2598 
2599  ARMARX_DEBUG << "clearing layer " << layerName;
2600 
2601  if (!hasLayer(layerName))
2602  {
2603  if (verbose)
2604  {
2605  ARMARX_VERBOSE << "Layer " << layerName
2606  << " can't be cleared, because it does not exist.";
2607  }
2608 
2609  return;
2610  }
2611 
2612  if (verbose)
2613  {
2614  ARMARX_VERBOSE << "Clearing layer " << layerName;
2615  }
2616 
2617  selectionNode->deselectAll();
2618 
2619  Layer& layer = layers.at(layerName);
2620 
2621  for (const auto& i : layer.addedCoordVisualizations)
2622  {
2623  removePoseVisu(layerName, i.first);
2624  }
2625 
2626  for (const auto& i : layer.addedLineVisualizations)
2627  {
2628  removeLineVisu(layerName, i.first);
2629  }
2630 
2631  for (const auto& i : layer.addedLineSetVisualizations)
2632  {
2633  removeLineSetVisu(layerName, i.first);
2634  }
2635 
2636  for (const auto& i : layer.addedBoxVisualizations)
2637  {
2638  removeBoxVisu(layerName, i.first);
2639  }
2640 
2641  for (const auto& i : layer.addedTextVisualizations)
2642  {
2643  removeTextVisu(layerName, i.first);
2644  }
2645 
2646  for (const auto& i : layer.addedSphereVisualizations)
2647  {
2648  removeSphereVisu(layerName, i.first);
2649  }
2650 
2651  for (const auto& i : layer.addedCylinderVisualizations)
2652  {
2653  removeCylinderVisu(layerName, i.first);
2654  }
2655 
2656  for (const auto& i : layer.addedCircleVisualizations)
2657  {
2658  removeCircleVisu(layerName, i.first);
2659  }
2660 
2661  for (const auto& i : layer.addedPointCloudVisualizations)
2662  {
2663  removePointCloudVisu(layerName, i.first);
2664  }
2665 
2666  for (const auto& i : layer.addedColoredPointCloudVisualizations)
2667  {
2668  removeColoredPointCloudVisu(layerName, i.first);
2669  }
2670 
2671  for (const auto& i : layer.addedPolygonVisualizations)
2672  {
2673  removePolygonVisu(layerName, i.first);
2674  }
2675 
2676  for (const auto& i : layer.addedTriMeshVisualizations)
2677  {
2678  removeTriMeshVisu(layerName, i.first);
2679  }
2680 
2681  for (const auto& i : layer.added24BitColoredPointCloudVisualizations)
2682  {
2683  remove24BitColoredPointCloudVisu(layerName, i.first);
2684  }
2685 
2686  for (const auto& i : layer.addedArrowVisualizations)
2687  {
2688  removeArrowVisu(layerName, i.first);
2689  }
2690 
2691  for (const auto& i : layer.addedRobotVisualizations)
2692  {
2693  removeRobotVisu(layerName, i.first);
2694  }
2695 
2696  for (const auto& i : layer.addedCustomVisualizations)
2697  {
2698  removeCustomVisu(layerName, i.first);
2699  }
2700  }
2701 
2702  void
2703  DebugDrawerComponent::clearDebugLayer(const Ice::Current&)
2704  {
2705  clearLayer(DEBUG_LAYER_NAME);
2706  }
2707 
2708  void
2709  DebugDrawerComponent::setMutex(RecursiveMutexPtr const& m)
2710  {
2711  mutex = m;
2712  }
2713 
2714  auto
2715  DebugDrawerComponent::getScopedVisuLock() -> RecursiveMutexLockPtr
2716  {
2717  RecursiveMutexLockPtr l;
2718 
2719  if (mutex)
2720  {
2721  //ARMARX_IMPORTANT << mutex.get();
2722  l.reset(new RecursiveMutexLock(*mutex));
2723  }
2724  else
2725  {
2726  ARMARX_IMPORTANT << deactivateSpam(5) << "NO 3D MUTEX!!!";
2727  }
2728 
2729  return l;
2730  }
2731 
2732  auto
2733  DebugDrawerComponent::getScopedAccumulatedDataLock() -> RecursiveMutexLockPtr
2734  {
2735  RecursiveMutexLockPtr l(new RecursiveMutexLock(*dataUpdateMutex));
2736  return l;
2737  }
2738 
2739  SoSeparator*
2740  DebugDrawerComponent::getVisualization()
2741  {
2742  return coinVisu;
2743  }
2744 
2745  DebugDrawerComponent::Layer&
2746  DebugDrawerComponent::requestLayer(const std::string& layerName)
2747  {
2748  auto l = getScopedVisuLock();
2749 
2750  if (hasLayer(layerName))
2751  {
2752  return layers.at(layerName);
2753  }
2754 
2755  ARMARX_VERBOSE << "Created layer " << layerName;
2756 
2757  SoSeparator* mainNode = new SoSeparator{};
2758  mainNode->ref();
2759  layers[layerName] = Layer();
2760  layers.at(layerName).mainNode = mainNode;
2761  layers.at(layerName).visible = getDefaultLayerVisibility(layerName);
2762  if (layers.at(layerName).visible)
2763  {
2764  layerMainNode->addChild(mainNode);
2765  }
2766  return layers.at(layerName);
2767  }
2768 
2769  void
2770  DebugDrawerComponent::updateVisualization()
2771  {
2772  auto lockData = getScopedAccumulatedDataLock();
2773  auto lockVisu = getScopedVisuLock();
2774  // check for clear&remove
2775  //(updates only contain elements to add for each layer after the last clear for this layer was executed)
2776  //this prevents a sequence add,clear,add to result in an empty layer
2777  for (const auto& layer : accumulatedUpdateData.clearLayers)
2778  {
2779  clearLayerQt(layer);
2780  }
2781  accumulatedUpdateData.clearLayers.clear();
2782 
2783  for (const auto& layer : accumulatedUpdateData.removeLayers)
2784  {
2785  removeLayerQt(layer);
2786  }
2787  accumulatedUpdateData.removeLayers.clear();
2788 
2789  //add elements
2790  for (auto& e : accumulatedUpdateData.coord)
2791  {
2792  drawCoordSystem(e.second);
2793  }
2794  accumulatedUpdateData.coord.clear();
2795 
2796  for (auto& e : accumulatedUpdateData.box)
2797  {
2798  drawBox(e.second);
2799  }
2800  accumulatedUpdateData.box.clear();
2801 
2802  for (auto& e : accumulatedUpdateData.line)
2803  {
2804  drawLine(e.second);
2805  }
2806  accumulatedUpdateData.line.clear();
2807 
2808  for (auto& e : accumulatedUpdateData.lineSet)
2809  {
2810  drawLineSet(e.second);
2811  }
2812  accumulatedUpdateData.lineSet.clear();
2813 
2814  for (auto& e : accumulatedUpdateData.sphere)
2815  {
2816  drawSphere(e.second);
2817  }
2818  accumulatedUpdateData.sphere.clear();
2819 
2820  for (auto& e : accumulatedUpdateData.cylinder)
2821  {
2822  drawCylinder(e.second);
2823  }
2824  accumulatedUpdateData.cylinder.clear();
2825 
2826  for (auto& e : accumulatedUpdateData.circle)
2827  {
2828  drawCircle(e.second);
2829  }
2830  accumulatedUpdateData.circle.clear();
2831 
2832  for (auto& e : accumulatedUpdateData.text)
2833  {
2834  drawText(e.second);
2835  }
2836  accumulatedUpdateData.text.clear();
2837 
2838  for (auto& e : accumulatedUpdateData.pointcloud)
2839  {
2840  drawPointCloud(e.second);
2841  }
2842  accumulatedUpdateData.pointcloud.clear();
2843 
2844  for (auto& e : accumulatedUpdateData.polygons)
2845  {
2846  drawPolygon(e.second);
2847  }
2848  accumulatedUpdateData.polygons.clear();
2849 
2850  for (auto& e : accumulatedUpdateData.arrows)
2851  {
2852  drawArrow(e.second);
2853  }
2854  accumulatedUpdateData.arrows.clear();
2855 
2856  for (auto& e : accumulatedUpdateData.triMeshes)
2857  {
2858  drawTriMesh(e.second);
2859  }
2860  accumulatedUpdateData.triMeshes.clear();
2861 
2862  for (auto& e : accumulatedUpdateData.robots)
2863  {
2864  ARMARX_DEBUG << "update visu / drawRobot for robot " << e.first;
2865  ensureExistingRobotNodes(e.second);
2866 
2867  drawRobot(e.second);
2868  e.second.nodeColors.clear();
2869  }
2870  accumulatedUpdateData.robots.clear();
2871 
2872  for (auto& e : accumulatedUpdateData.coloredpointcloud)
2873  {
2874  drawColoredPointCloud(e.second);
2875  }
2876  accumulatedUpdateData.coloredpointcloud.clear();
2877 
2878  for (auto& e : accumulatedUpdateData.colored24Bitpointcloud)
2879  {
2880  draw24BitColoredPointCloud(e.second);
2881  }
2882  accumulatedUpdateData.colored24Bitpointcloud.clear();
2883 
2884  onUpdateVisualization();
2885  }
2886 
2887  bool
2888  DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
2889  {
2890  auto l = getScopedVisuLock();
2891  return layers.find(layerName) != layers.end();
2892  }
2893 
2894  void
2895  DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
2896  {
2897  auto lockData = getScopedAccumulatedDataLock();
2898  //updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
2899  //this prevents a sequence add,clear,add to result in an empty layer
2900  //=>remove all objects pending for this layer
2901  removeAccumulatedData(layerName);
2902  accumulatedUpdateData.removeLayers.emplace(layerName);
2903  }
2904 
2905  void
2906  DebugDrawerComponent::removeLayerQt(const std::string& layerName)
2907  {
2908  auto lockData = getScopedAccumulatedDataLock();
2909  auto lockVisu = getScopedVisuLock();
2910  if (!hasLayer(layerName))
2911  {
2912  if (verbose)
2913  {
2914  ARMARX_VERBOSE << "Layer " << layerName
2915  << " can't be removed, because it does not exist.";
2916  }
2917 
2918  return;
2919  }
2920 
2921  if (verbose)
2922  {
2923  ARMARX_VERBOSE << "Removing layer " << layerName;
2924  }
2925 
2926  clearLayerQt(layerName);
2927  {
2928  auto& layer = layers.at(layerName);
2929  layerMainNode->removeChild(layer.mainNode);
2930  layer.mainNode->unref();
2931  layers.erase(layerName);
2932  }
2933  }
2934 
2935  template <class UpdateDataType>
2936  void
2937  removeUpdateDataFromMap(const std::string& layerName,
2938  std::map<std::string, UpdateDataType>& map)
2939  {
2940  auto it = map.begin();
2941  while (it != map.end())
2942  {
2943  auto curr = it++;
2944  if (curr->second.layerName == layerName)
2945  {
2946  map.erase(curr);
2947  }
2948  }
2949  }
2950 
2951  void
2952  DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
2953  {
2954  auto lockData = getScopedAccumulatedDataLock();
2955 
2956  removeUpdateDataFromMap(layerName, accumulatedUpdateData.coord);
2957  removeUpdateDataFromMap(layerName, accumulatedUpdateData.line);
2958  removeUpdateDataFromMap(layerName, accumulatedUpdateData.lineSet);
2959  removeUpdateDataFromMap(layerName, accumulatedUpdateData.box);
2960  removeUpdateDataFromMap(layerName, accumulatedUpdateData.text);
2961  removeUpdateDataFromMap(layerName, accumulatedUpdateData.sphere);
2962  removeUpdateDataFromMap(layerName, accumulatedUpdateData.cylinder);
2963  removeUpdateDataFromMap(layerName, accumulatedUpdateData.pointcloud);
2964  removeUpdateDataFromMap(layerName, accumulatedUpdateData.coloredpointcloud);
2965  removeUpdateDataFromMap(layerName, accumulatedUpdateData.colored24Bitpointcloud);
2966  removeUpdateDataFromMap(layerName, accumulatedUpdateData.polygons);
2967  removeUpdateDataFromMap(layerName, accumulatedUpdateData.arrows);
2968  removeUpdateDataFromMap(layerName, accumulatedUpdateData.robots);
2969  removeUpdateDataFromMap(layerName, accumulatedUpdateData.triMeshes);
2970 
2971  onRemoveAccumulatedData(layerName);
2972  }
2973 
2974  void
2975  DebugDrawerComponent::enableLayerVisu(const std::string& layerName,
2976  bool visible,
2977  const ::Ice::Current&)
2978  {
2979  setLayerVisibility(layerName, visible);
2980  }
2981 
2982  void
2983  DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
2984  {
2985  enableLayerVisu(DEBUG_LAYER_NAME, visible);
2986  }
2987 
2988  Ice::StringSeq
2989  DebugDrawerComponent::layerNames(const ::Ice::Current&)
2990  {
2991  auto l = getScopedVisuLock();
2992  Ice::StringSeq seq{};
2993 
2994  for (const auto& layer : layers)
2995  {
2996  seq.push_back(layer.first);
2997  }
2998 
2999  return seq;
3000  }
3001 
3002  ::armarx::LayerInformationSequence
3003  DebugDrawerComponent::layerInformation(const ::Ice::Current&)
3004  {
3005  ::armarx::LayerInformationSequence seq{};
3006  auto l = getScopedVisuLock();
3007 
3008  for (const auto& layer : layers)
3009  {
3010  int count = layer.second.addedCoordVisualizations.size() +
3011  layer.second.addedLineVisualizations.size() +
3012  layer.second.addedLineSetVisualizations.size() +
3013  layer.second.addedBoxVisualizations.size() +
3014  layer.second.addedTextVisualizations.size() +
3015  layer.second.addedSphereVisualizations.size() +
3016  layer.second.addedCylinderVisualizations.size() +
3017  layer.second.addedCircleVisualizations.size() +
3018  layer.second.addedPointCloudVisualizations.size() +
3019  layer.second.addedColoredPointCloudVisualizations.size() +
3020  layer.second.added24BitColoredPointCloudVisualizations.size() +
3021  layer.second.addedPolygonVisualizations.size() +
3022  layer.second.addedTriMeshVisualizations.size() +
3023  layer.second.addedArrowVisualizations.size() +
3024  layer.second.addedRobotVisualizations.size() +
3025  layer.second.addedCustomVisualizations.size();
3026  ::armarx::LayerInformation info = {layer.first, layer.second.visible, count};
3027  seq.push_back(info);
3028  }
3029 
3030  return seq;
3031  }
3032 
3033  void
3034  DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData& d)
3035  {
3036  auto l = getScopedVisuLock();
3037 
3038  auto& layer = requestLayer(d.layerName);
3039 
3040  removeColoredPointCloud(d.layerName, d.name);
3041 
3042  if (!d.active)
3043  {
3044  return;
3045  }
3046 
3047  const auto& pcl = d.pointCloud.points;
3048 
3049  SoSeparator* pclSep = new SoSeparator;
3050 
3051  SoMaterial* pclMat = new SoMaterial;
3052  std::vector<SbColor> colors;
3053  colors.reserve(pcl.size());
3054  std::transform(pcl.begin(),
3055  pcl.end(),
3056  std::back_inserter(colors),
3057  [](const DebugDrawerColoredPointCloudElement& elem) {
3058  return SbColor{elem.color.r, elem.color.g, elem.color.b};
3059  });
3060  pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
3061  pclMat->ambientColor.setValues(0, colors.size(), colors.data());
3062  pclSep->addChild(pclMat);
3063 
3064  SoMaterialBinding* pclMatBind = new SoMaterialBinding;
3065  pclMatBind->value = SoMaterialBinding::PER_PART;
3066  pclSep->addChild(pclMatBind);
3067 
3068  SoCoordinate3* pclCoords = new SoCoordinate3;
3069  std::vector<SbVec3f> coords;
3070  coords.reserve(pcl.size());
3071  std::transform(pcl.begin(),
3072  pcl.end(),
3073  std::back_inserter(coords),
3074  [](const DebugDrawerColoredPointCloudElement& elem) {
3075  return SbVec3f{elem.x, elem.y, elem.z};
3076  });
3077  pclCoords->point.setValues(0, coords.size(), coords.data());
3078  pclSep->addChild(pclCoords);
3079 
3080  SoDrawStyle* pclStye = new SoDrawStyle;
3081  pclStye->pointSize = d.pointCloud.pointSize;
3082  pclSep->addChild(pclStye);
3083 
3084  pclSep->addChild(new SoPointSet);
3085 
3086  ARMARX_DEBUG << d.name << "PointCloud Update " << pcl.size();
3087 
3088  layer.addedColoredPointCloudVisualizations[d.name] = pclSep;
3089  layer.mainNode->addChild(pclSep);
3090  }
3091 
3092  void
3093  DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName,
3094  const std::string& name)
3095  {
3096  auto l = getScopedVisuLock();
3097 
3098  if (!hasLayer(layerName))
3099  {
3100  return;
3101  }
3102 
3103  auto& layer = layers.at(layerName);
3104 
3105  if (layer.addedColoredPointCloudVisualizations.find(name) ==
3106  layer.addedColoredPointCloudVisualizations.end())
3107  {
3108  return;
3109  }
3110 
3111  layer.mainNode->removeChild(layer.addedColoredPointCloudVisualizations[name]);
3112  layer.addedColoredPointCloudVisualizations.erase(name);
3113  }
3114 
3115  void
3116  DebugDrawerComponent::setColoredPointCloudVisu(const std::string& layerName,
3117  const std::string& pointCloudName,
3118  const DebugDrawerColoredPointCloud& pointCloud,
3119  const Ice::Current&)
3120  {
3121  {
3122  auto l = getScopedAccumulatedDataLock();
3123  std::string entryName = simox::alg::replace_all(
3124  std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
3125  ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
3126  d.pointCloud = pointCloud;
3127  d.layerName = layerName;
3128  d.name = pointCloudName;
3129  d.active = true;
3130  }
3131  }
3132 
3133  void
3134  DebugDrawerComponent::setColoredPointCloudDebugLayerVisu(
3135  const std::string& pointCloudName,
3136  const DebugDrawerColoredPointCloud& pointCloud,
3137  const Ice::Current&)
3138  {
3139  setColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
3140  }
3141 
3142  void
3143  DebugDrawerComponent::removeColoredPointCloudVisu(const std::string& layerName,
3144  const std::string& pointCloudName,
3145  const Ice::Current&)
3146  {
3147  {
3148  auto l = getScopedAccumulatedDataLock();
3149  std::string entryName = simox::alg::replace_all(
3150  std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
3151  ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
3152  d.layerName = layerName;
3153  d.name = pointCloudName;
3154  d.active = false;
3155  }
3156  }
3157 
3158  void
3159  DebugDrawerComponent::removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName,
3160  const Ice::Current&)
3161  {
3162  removeColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
3163  }
3164 
3165  void
3166  DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
3167  {
3168  auto l = getScopedVisuLock();
3169 
3170  auto& layer = requestLayer(d.layerName);
3171 
3172  remove24BitColoredPointCloud(d.layerName, d.name);
3173 
3174  if (!d.active)
3175  {
3176  return;
3177  }
3178 
3179  const auto& pcl = d.pointCloud.points;
3180 
3181  SoSeparator* pclSep = new SoSeparator;
3182 
3183  SoMaterial* pclMat = new SoMaterial;
3184  std::vector<SbColor> colors;
3185  colors.reserve(pcl.size());
3186  std::transform(pcl.begin(),
3187  pcl.end(),
3188  std::back_inserter(colors),
3189  [](const DebugDrawer24BitColoredPointCloudElement& elem)
3190  {
3191  return SbColor{static_cast<float>(elem.color.r) / 255.f,
3192  static_cast<float>(elem.color.g) / 255.f,
3193  static_cast<float>(elem.color.b) / 255.f};
3194  });
3195  pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
3196  pclMat->ambientColor.setValues(0, colors.size(), colors.data());
3197  pclMat->transparency = d.pointCloud.transparency;
3198  pclSep->addChild(pclMat);
3199 
3200  SoMaterialBinding* pclMatBind = new SoMaterialBinding;
3201  pclMatBind->value = SoMaterialBinding::PER_PART;
3202  pclSep->addChild(pclMatBind);
3203 
3204  SoCoordinate3* pclCoords = new SoCoordinate3;
3205  std::vector<SbVec3f> coords;
3206  coords.reserve(pcl.size());
3207  std::transform(pcl.begin(),
3208  pcl.end(),
3209  std::back_inserter(coords),
3210  [](const DebugDrawer24BitColoredPointCloudElement& elem) {
3211  return SbVec3f{elem.x, elem.y, elem.z};
3212  });
3213  pclCoords->point.setValues(0, coords.size(), coords.data());
3214  pclSep->addChild(pclCoords);
3215 
3216  SoDrawStyle* pclStye = new SoDrawStyle;
3217  pclStye->pointSize = d.pointCloud.pointSize;
3218  pclSep->addChild(pclStye);
3219 
3220  pclSep->addChild(new SoPointSet);
3221 
3222  ARMARX_DEBUG << d.name << "PointCloud Update " << pcl.size();
3223 
3224  layer.added24BitColoredPointCloudVisualizations[d.name] = pclSep;
3225  layer.mainNode->addChild(pclSep);
3226  }
3227 
3228  void
3229  DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName,
3230  const std::string& name)
3231  {
3232  auto l = getScopedVisuLock();
3233 
3234  if (!hasLayer(layerName))
3235  {
3236  return;
3237  }
3238 
3239  auto& layer = layers.at(layerName);
3240 
3241  if (layer.added24BitColoredPointCloudVisualizations.find(name) ==
3242  layer.added24BitColoredPointCloudVisualizations.end())
3243  {
3244  return;
3245  }
3246 
3247  layer.mainNode->removeChild(layer.added24BitColoredPointCloudVisualizations[name]);
3248  layer.added24BitColoredPointCloudVisualizations.erase(name);
3249  }
3250 
3251  void
3252  DebugDrawerComponent::set24BitColoredPointCloudVisu(
3253  const std::string& layerName,
3254  const std::string& pointCloudName,
3255  const DebugDrawer24BitColoredPointCloud& pointCloud,
3256  const Ice::Current&)
3257  {
3258  {
3259  auto l = getScopedAccumulatedDataLock();
3260  std::string entryName = simox::alg::replace_all(
3261  std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
3262  Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
3263  d.pointCloud = pointCloud;
3264  d.layerName = layerName;
3265  d.name = pointCloudName;
3266  d.active = true;
3267  }
3268  }
3269 
3270  void
3271  DebugDrawerComponent::set24BitColoredPointCloudDebugLayerVisu(
3272  const std::string& pointCloudName,
3273  const DebugDrawer24BitColoredPointCloud& pointCloud,
3274  const Ice::Current&)
3275  {
3276  set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
3277  }
3278 
3279  void
3280  DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName,
3281  const std::string& pointCloudName,
3282  const Ice::Current&)
3283  {
3284  {
3285  auto l = getScopedAccumulatedDataLock();
3286  std::string entryName = simox::alg::replace_all(
3287  std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
3288  Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
3289  d.layerName = layerName;
3290  d.name = pointCloudName;
3291  d.active = false;
3292  }
3293  }
3294 
3295  void
3296  DebugDrawerComponent::remove24BitColoredPointCloudDebugLayerVisu(
3297  const std::string& pointCloudName,
3298  const Ice::Current&)
3299  {
3300  remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
3301  }
3302 
3303  void
3304  DebugDrawerComponent::setCircleArrowVisu(const std::string& layerName,
3305  const std::string& circleName,
3306  const Vector3BasePtr& globalPosition,
3307  const Vector3BasePtr& directionVec,
3308  Ice::Float radius,
3309  Ice::Float circleCompletion,
3310  Ice::Float width,
3311  const DrawColor& color,
3312  const Ice::Current&)
3313  {
3314 
3315  auto l = getScopedAccumulatedDataLock();
3316  std::string entryName = simox::alg::replace_all(
3317  std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
3318  CircleData& d = accumulatedUpdateData.circle[entryName];
3319  d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
3320  d.direction = Vector3Ptr::dynamicCast(directionVec)->toEigen();
3321  d.circleCompletion = circleCompletion;
3322  d.radius = radius;
3323  d.width = width;
3324  d.layerName = layerName;
3325  d.name = circleName;
3326  d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
3327  d.active = true;
3328  }
3329 
3330  void
3331  DebugDrawerComponent::setCircleDebugLayerVisu(const std::string& circleName,
3332  const Vector3BasePtr& globalPosition,
3333  const Vector3BasePtr& directionVec,
3334  Ice::Float radius,
3335  Ice::Float circleCompletion,
3336  Ice::Float width,
3337  const DrawColor& color,
3338  const Ice::Current& c)
3339  {
3340  setCircleArrowVisu(DEBUG_LAYER_NAME,
3341  circleName,
3342  globalPosition,
3343  directionVec,
3344  radius,
3345  circleCompletion,
3346  width,
3347  color,
3348  c);
3349  }
3350 
3351  void
3352  DebugDrawerComponent::removeCircleVisu(const std::string& layerName,
3353  const std::string& circleName,
3354  const Ice::Current&)
3355  {
3356 
3357  auto l = getScopedAccumulatedDataLock();
3358  std::string entryName = simox::alg::replace_all(
3359  std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
3360  CircleData& d = accumulatedUpdateData.circle[entryName];
3361  d.layerName = layerName;
3362  d.name = circleName;
3363  d.active = false;
3364  }
3365 
3366  void
3367  DebugDrawerComponent::removeCircleDebugLayerVisu(const std::string& circleName,
3368  const Ice::Current& c)
3369  {
3370  removeCircleVisu(DEBUG_LAYER_NAME, circleName, c);
3371  }
3372 } // namespace armarx
armarx::DebugDrawerComponent::LineData::scale
float scale
Definition: DebugDrawerComponent.h:592
DebugDrawerComponent.h
armarx::GetOrthonormalVectors
Eigen::Vector3f GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f &dir1, Eigen::Vector3f &dir2)
Definition: DebugDrawerComponent.cpp:867
armarx::DebugDrawerComponent::Layer
Contains data for a layer.
Definition: DebugDrawerComponent.h:797
armarx::DebugDrawerComponent::removeSelectionCallbacks
void removeSelectionCallbacks()
Definition: DebugDrawerComponent.cpp:263
armarx::DebugDrawerComponent::drawPointCloud
void drawPointCloud(const PointCloudData &d)
Definition: DebugDrawerComponent.cpp:934
pcl
Definition: pcl_point_operators.cpp:3
armarx::DebugDrawerComponent::CircleData::circleCompletion
float circleCompletion
Definition: DebugDrawerComponent.h:640
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::DebugDrawerComponent::ColoredPointCloudData
Definition: DebugDrawerComponent.h:649
armarx::DebugDrawerComponent::Layer::added24BitColoredPointCloudVisualizations
std::map< std::string, SoSeparator * > added24BitColoredPointCloudVisualizations
Definition: DebugDrawerComponent.h:810
armarx::DebugDrawerComponent::selectionMutex
static std::recursive_mutex selectionMutex
Definition: DebugDrawerComponent.h:859
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:919
armarx::DebugDrawerComponent::RobotData::updatePose
bool updatePose
Definition: DebugDrawerComponent.h:699
armarx::DebugDrawerComponent
The DebugDrawerComponent class.
Definition: DebugDrawerComponent.h:113
armarx::DebugDrawerComponent::PolygonData::colorBorder
VirtualRobot::VisualizationFactory::Color colorBorder
Definition: DebugDrawerComponent.h:664
armarx::DebugDrawerComponent::Layer::addedPolygonVisualizations
std::map< std::string, SoSeparator * > addedPolygonVisualizations
Definition: DebugDrawerComponent.h:811
armarx::DebugDrawerComponent::BoxData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:607
armarx::DebugDrawerComponent::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: DebugDrawerComponent.cpp:382
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::DebugDrawerComponent::Colored24BitPointCloudData::pointCloud
DebugDrawer24BitColoredPointCloud pointCloud
Definition: DebugDrawerComponent.h:656
armarx::CMakePackageFinder::packageFound
bool packageFound() const
Returns whether or not this package was found with cmake.
Definition: CMakePackageFinder.cpp:511
boost::shared_ptr< VirtualRobot::CoinVisualization >
armarx::DebugDrawerComponent::TextData::position
Eigen::Vector3f position
Definition: DebugDrawerComponent.h:613
armarx::DebugDrawerComponent::Layer::addedCircleVisualizations
std::map< std::string, SoSeparator * > addedCircleVisualizations
Definition: DebugDrawerComponent.h:807
armarx::DebugDrawerComponent::RobotData::drawStyle
armarx::DrawStyle drawStyle
Definition: DebugDrawerComponent.h:705
armarx::DebugDrawerComponent::CylinderData::radius
float radius
Definition: DebugDrawerComponent.h:630
armarx::DebugDrawerComponent::RobotData::configuration
std::map< std::string, float > configuration
Definition: DebugDrawerComponent.h:703
index
uint8_t index
Definition: EtherCATFrame.h:59
VirtualRobot
Definition: FramedPose.h:42
armarx::DebugDrawerComponent::ArrowData::width
float width
Definition: DebugDrawerComponent.h:677
armarx::DebugDrawerComponent::TextData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:614
armarx::DebugDrawerComponent::getScopedVisuLock
RecursiveMutexLockPtr getScopedVisuLock()
getScopedLock If using the coin visualization it must be ensured that all rendering calls are protect...
Definition: DebugDrawerComponent.cpp:2715
armarx::DebugDrawerComponent::reportSelectionChanged
void reportSelectionChanged(const DebugDrawerSelectionList &selectedElements, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:271
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::DebugDrawerComponent::Layer::addedColoredPointCloudVisualizations
std::map< std::string, SoSeparator * > addedColoredPointCloudVisualizations
Definition: DebugDrawerComponent.h:809
armarx::DebugDrawerComponent::Layer::addedLineVisualizations
std::map< std::string, SoSeparator * > addedLineVisualizations
Definition: DebugDrawerComponent.h:801
armarx::DebugDrawerComponent::selectableLayers
std::set< std::string > selectableLayers
Definition: DebugDrawerComponent.h:860
armarx::DebugDrawerComponent::verbose
bool verbose
Definition: DebugDrawerComponent.h:839
armarx::DebugDrawerComponent::updateVisualization
void updateVisualization()
Definition: DebugDrawerComponent.cpp:2770
armarx::DebugDrawerComponent::Layer::addedCoordVisualizations
std::map< std::string, SoSeparator * > addedCoordVisualizations
Definition: DebugDrawerComponent.h:800
armarx::DebugDrawerComponent::CylinderData
Definition: DebugDrawerComponent.h:625
armarx::DebugDrawerComponent::deselect
void deselect(const std::string &layerName, const std::string &elementName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:212
armarx::DebugDrawerComponent::DrawData::update
bool update
Whether an existing visu should be updated.
Definition: DebugDrawerComponent.h:579
armarx::DebugDrawerComponent::BoxData::globalPose
Eigen::Matrix4f globalPose
Definition: DebugDrawerComponent.h:603
SELECTION_NAME_SPLITTER
#define SELECTION_NAME_SPLITTER
Definition: DebugDrawerComponent.cpp:70
armarx::DebugDrawerComponent::ArrowData
Definition: DebugDrawerComponent.h:672
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::DebugDrawerComponent::LineData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:593
armarx::DebugDrawerComponent::timerSensor
SoTimerSensor * timerSensor
Definition: DebugDrawerComponent.h:868
armarx::DebugDrawerComponent::drawCircle
void drawCircle(const DebugDrawerComponent::CircleData &d)
Definition: DebugDrawerComponent.cpp:889
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::DebugDrawerComponent::hasLayer
bool hasLayer(const std::string &layerName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:2888
armarx::DebugDrawerComponent::removeLineSet
void removeLineSet(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1426
SELECTION_NAME
#define SELECTION_NAME(layerName, elementName)
Definition: DebugDrawerComponent.cpp:71
armarx::DebugDrawerComponent::SphereData::position
Eigen::Vector3f position
Definition: DebugDrawerComponent.h:620
armarx::DebugDrawerComponent::Layer::addedArrowVisualizations
std::map< std::string, SoSeparator * > addedArrowVisualizations
Definition: DebugDrawerComponent.h:813
armarx::DebugDrawerComponent::RobotData::updateNodeColor
bool updateNodeColor
Definition: DebugDrawerComponent.h:696
armarx::DebugDrawerComponent::getSelections
DebugDrawerSelectionList getSelections(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:296
armarx::DebugDrawerComponent::PolygonData::colorInner
VirtualRobot::VisualizationFactory::Color colorInner
Definition: DebugDrawerComponent.h:663
armarx::DebugDrawerComponent::exportLayer
void exportLayer(const std::string &filename, const std::string &layerName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:508
armarx::DebugDrawerComponent::DrawData::name
std::string name
Definition: DebugDrawerComponent.h:574
armarx::DebugDrawerComponent::topicMutex
RecursiveMutexPtr topicMutex
Definition: DebugDrawerComponent.h:857
armarx::DebugDrawerComponent::updateVisualizationCB
static void updateVisualizationCB(void *data, SoSensor *sensor)
Definition: DebugDrawerComponent.cpp:411
armarx::DebugDrawerComponent::removeCoordSystem
void removeCoordSystem(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1700
armarx::DebugDrawerComponent::Layer::addedTextVisualizations
std::map< std::string, SoSeparator * > addedTextVisualizations
Definition: DebugDrawerComponent.h:804
armarx::DebugDrawerComponent::coinVisu
SoSeparator * coinVisu
Definition: DebugDrawerComponent.h:836
ColorUtils.h
armarx::DebugDrawerComponent::removeSphere
void removeSphere(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1489
armarx::DebugDrawerComponent::PointCloudData
Definition: DebugDrawerComponent.h:644
armarx::DebugDrawerComponent::RecursiveMutexLock
std::unique_lock< RecursiveMutex > RecursiveMutexLock
Definition: DebugDrawerComponent.h:497
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:366
armarx::DebugDrawerComponent::RobotData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:694
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::DebugDrawerComponent::drawCylinder
void drawCylinder(const CylinderData &d)
Definition: DebugDrawerComponent.cpp:828
armarx::DebugDrawerComponent::RobotData::updateColor
bool updateColor
Definition: DebugDrawerComponent.h:693
armarx::DebugDrawerComponent::RobotData
Definition: DebugDrawerComponent.h:681
armarx::DebugDrawerComponent::Layer::addedCylinderVisualizations
std::map< std::string, SoSeparator * > addedCylinderVisualizations
Definition: DebugDrawerComponent.h:806
armarx::DebugDrawerComponent::CoordData
Definition: DebugDrawerComponent.h:582
armarx::DebugDrawerComponent::deselectionCallback
void deselectionCallback()
Definition: DebugDrawerComponent.cpp:248
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::DebugDrawerComponent::Layer::addedCustomVisualizations
std::map< std::string, SoSeparator * > addedCustomVisualizations
Definition: DebugDrawerComponent.h:816
armarx::DebugDrawerComponent::LineData::p1
Eigen::Vector3f p1
Definition: DebugDrawerComponent.h:590
armarx::DebugDrawerComponent::SphereData
Definition: DebugDrawerComponent.h:618
armarx::DebugDrawerComponent::CircleData::radius
float radius
Definition: DebugDrawerComponent.h:638
armarx::DebugDrawerComponent::CylinderData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:631
armarx::DebugDrawerComponent::Layer::addedBoxVisualizations
std::map< std::string, SoSeparator * > addedBoxVisualizations
Definition: DebugDrawerComponent.h:803
armarx::DebugDrawerComponent::SphereData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:621
armarx::DebugDrawerComponent::BoxData::height
float height
Definition: DebugDrawerComponent.h:605
armarx::DebugDrawerComponent::DrawData::active
bool active
Definition: DebugDrawerComponent.h:575
armarx::DebugDrawerComponent::Layer::addedPointCloudVisualizations
std::map< std::string, SoSeparator * > addedPointCloudVisualizations
Definition: DebugDrawerComponent.h:808
armarx::DebugDrawerComponent::RobotData::updateConfig
bool updateConfig
Definition: DebugDrawerComponent.h:702
armarx::DebugDrawerComponent::enableSelections
void enableSelections(const std::string &layerName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:130
armarx::DebugDrawerComponent::TextData
Definition: DebugDrawerComponent.h:610
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
armarx::DebugDrawerComponent::CircleData::width
float width
Definition: DebugDrawerComponent.h:639
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::DebugDrawerComponent::RecursiveMutex
std::recursive_mutex RecursiveMutex
Definition: DebugDrawerComponent.h:495
armarx::selection_callback
void selection_callback(void *userdata, SoPath *path)
Definition: DebugDrawerComponent.cpp:83
armarx::DebugDrawerComponent::TriMeshData
Definition: DebugDrawerComponent.h:667
armarx::DebugDrawerComponent::removeBox
void removeBox(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1447
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:194
armarx::DebugDrawerComponent::listenerPrx
DebugDrawerListenerPrx listenerPrx
Definition: DebugDrawerComponent.h:864
armarx::DebugDrawerComponent::ArrowData::length
float length
Definition: DebugDrawerComponent.h:676
armarx::DebugDrawerComponent::CylinderData::length
float length
Definition: DebugDrawerComponent.h:629
enabled
std::atomic< bool > * enabled
Definition: RemoteGuiWidgetController.cpp:75
armarx::DebugDrawerComponent::TextData::text
std::string text
Definition: DebugDrawerComponent.h:612
armarx::DebugDrawerComponent::CylinderData::direction
Eigen::Vector3f direction
Definition: DebugDrawerComponent.h:628
armarx::DebugDrawerComponent::RobotData::nodeColors
std::map< std::string, VirtualRobot::VisualizationFactory::Color > nodeColors
Definition: DebugDrawerComponent.h:697
armarx::DebugDrawerComponent::PolygonData::points
std::vector< Eigen::Vector3f > points
Definition: DebugDrawerComponent.h:661
armarx::DebugDrawerComponent::timerSensorMutex
std::mutex timerSensorMutex
Definition: DebugDrawerComponent.h:869
filename
std::string filename
Definition: VisualizationRobot.cpp:86
armarx::DebugDrawerComponent::exportScene
void exportScene(const std::string &filename, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:486
armarx::DebugDrawerComponent::CircleData
Definition: DebugDrawerComponent.h:634
armarx::DebugDrawerComponent::ColoredPointCloudData::pointCloud
DebugDrawerColoredPointCloud pointCloud
Definition: DebugDrawerComponent.h:651
max
T max(T t1, T t2)
Definition: gdiam.h:51
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::removeUpdateDataFromMap
void removeUpdateDataFromMap(const std::string &layerName, std::map< std::string, UpdateDataType > &map)
Definition: DebugDrawerComponent.cpp:2937
armarx::DebugDrawerComponent::BoxData
Definition: DebugDrawerComponent.h:601
armarx::deselection_callback
void deselection_callback(void *userdata, SoPath *path)
Definition: DebugDrawerComponent.cpp:92
armarx::DebugDrawerComponent::requestLayer
Layer & requestLayer(const std::string &layerName)
If a layer with layerName exists it is returned. Otherwise a new layer with given name is created and...
Definition: DebugDrawerComponent.cpp:2746
armarx::DebugDrawerComponent::PointCloudData::pointCloud
DebugDrawerPointCloud pointCloud
Definition: DebugDrawerComponent.h:646
armarx::DebugDrawerComponent::removeCylinder
void removeCylinder(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1510
armarx::DebugDrawerComponent::CircleData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:641
armarx::DebugDrawerComponent::cycleTimeMS
float cycleTimeMS
Definition: DebugDrawerComponent.h:866
armarx::DebugDrawerComponent::RobotData::armarxProject
std::string armarxProject
Definition: DebugDrawerComponent.h:691
armarx::DebugDrawerComponent::Layer::addedSphereVisualizations
std::map< std::string, SoSeparator * > addedSphereVisualizations
Definition: DebugDrawerComponent.h:805
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
armarx::DebugDrawerComponent::removeCircle
void removeCircle(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1531
armarx::DebugDrawerComponent::RecursiveMutexPtr
std::shared_ptr< RecursiveMutex > RecursiveMutexPtr
Definition: DebugDrawerComponent.h:496
armarx::DebugDrawerComponent::PolygonData
Definition: DebugDrawerComponent.h:659
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:254
CMakePackageFinder.h
armarx::DebugDrawerComponent::RobotData::robotFile
std::string robotFile
Definition: DebugDrawerComponent.h:690
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::DebugDrawerComponent::drawLine
void drawLine(const LineData &d)
Definition: DebugDrawerComponent.cpp:571
armarx::DebugDrawerComponent::RobotData::globalPose
Eigen::Matrix4f globalPose
Definition: DebugDrawerComponent.h:700
armarx::DebugDrawerComponent::Layer::addedLineSetVisualizations
std::map< std::string, SoSeparator * > addedLineSetVisualizations
Definition: DebugDrawerComponent.h:802
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::ArmarXDataPath::addDataPaths
static void addDataPaths(const std::string &dataPathList)
Definition: ArmarXDataPath.cpp:554
armarx::DebugDrawerComponent::removeText
void removeText(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1468
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:300
armarx::DebugDrawerComponent::CircleData::position
Eigen::Vector3f position
Definition: DebugDrawerComponent.h:636
armarx::DebugDrawerComponent::CylinderData::position
Eigen::Vector3f position
Definition: DebugDrawerComponent.h:627
armarx::DebugDrawerComponent::installSelectionCallbacks
void installSelectionCallbacks()
Definition: DebugDrawerComponent.cpp:255
armarx::DebugDrawerComponent::CircleData::direction
Eigen::Vector3f direction
Definition: DebugDrawerComponent.h:637
armarx::DebugDrawerComponent::BoxData::depth
float depth
Definition: DebugDrawerComponent.h:606
armarx::DebugDrawerComponent::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: DebugDrawerComponent.cpp:425
armarx::DebugDrawerComponent::LineSetData::lineSet
DebugDrawerLineSet lineSet
Definition: DebugDrawerComponent.h:598
armarx::DebugDrawerComponent::Layer::addedTriMeshVisualizations
std::map< std::string, SoSeparator * > addedTriMeshVisualizations
Definition: DebugDrawerComponent.h:812
armarx::DebugDrawerComponent::LineData
Definition: DebugDrawerComponent.h:588
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
armarx::DebugDrawerComponent::drawBox
void drawBox(const BoxData &d)
Definition: DebugDrawerComponent.cpp:702
armarx::DebugDrawerComponent::removePointCloud
void removePointCloud(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1552
armarx::DebugDrawerComponent::LineData::p2
Eigen::Vector3f p2
Definition: DebugDrawerComponent.h:591
armarx::DebugDrawerComponent::RecursiveMutexLockPtr
std::shared_ptr< RecursiveMutexLock > RecursiveMutexLockPtr
Definition: DebugDrawerComponent.h:498
armarx::DebugDrawerComponent::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: DebugDrawerComponent.cpp:453
armarx::colorutils::HeatMapRGBAColor
DrawColor HeatMapRGBAColor(float percentage)
Definition: ColorUtils.h:175
armarx::DebugDrawerComponent::CoordData::globalPose
Eigen::Matrix4f globalPose
Definition: DebugDrawerComponent.h:584
armarx::DebugDrawerComponent::selectionCallback
void selectionCallback()
Definition: DebugDrawerComponent.cpp:241
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::DebugDrawerComponent::drawLineSet
void drawLineSet(const LineSetData &d)
Definition: DebugDrawerComponent.cpp:593
armarx::DebugDrawerComponent::ArrowData::color
VirtualRobot::VisualizationFactory::Color color
Definition: DebugDrawerComponent.h:678
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::DebugDrawerComponent::TriMeshData::triMesh
DebugDrawerTriMesh triMesh
Definition: DebugDrawerComponent.h:669
SELECTION_NAME_PREFIX
#define SELECTION_NAME_PREFIX
Definition: DebugDrawerComponent.cpp:68
armarx::DebugDrawerComponent::drawSphere
void drawSphere(const SphereData &d)
Definition: DebugDrawerComponent.cpp:784
armarx::DebugDrawerComponent::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: DebugDrawerComponent.cpp:434
ArmarXDataPath.h
armarx::DebugDrawerComponent::ArrowData::position
Eigen::Vector3f position
Definition: DebugDrawerComponent.h:674
armarx::DebugDrawerComponent::SphereData::radius
float radius
Definition: DebugDrawerComponent.h:622
armarx::DebugDrawerComponent::DrawData::layerName
std::string layerName
Definition: DebugDrawerComponent.h:573
armarx::DebugDrawerComponent::setVisuUpdateTime
void setVisuUpdateTime(float visuUpdatesPerSec)
setVisuUpdateTime Specifies how often the accumulated draw commands should be applied to the visualiz...
Definition: DebugDrawerComponent.cpp:354
armarx::DebugDrawerComponent::LineSetData
Definition: DebugDrawerComponent.h:596
armarx::DebugDrawerComponent::~DebugDrawerComponent
~DebugDrawerComponent() override
Definition: DebugDrawerComponent.cpp:372
armarx::DebugDrawerComponent::PolygonData::lineWidth
float lineWidth
Definition: DebugDrawerComponent.h:662
armarx::DebugDrawerComponent::layerMainNode
SoSeparator * layerMainNode
Main node for all layers.
Definition: DebugDrawerComponent.h:844
armarx::DebugDrawerComponent::drawCoordSystem
void drawCoordSystem(const CoordData &d)
Definition: DebugDrawerComponent.cpp:538
armarx::DebugDrawerComponent::clearSelections
void clearSelections(const std::string &layerName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:155
armarx::DebugDrawerComponent::TextData::size
int size
Definition: DebugDrawerComponent.h:615
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::DebugDrawerComponent::disableSelections
void disableSelections(const std::string &layerName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:139
armarx::DebugDrawerComponent::CoordData::scale
float scale
Definition: DebugDrawerComponent.h:585
armarx::DebugDrawerComponent::Colored24BitPointCloudData
Definition: DebugDrawerComponent.h:654
armarx::DebugDrawerComponent::drawText
void drawText(const TextData &d)
Definition: DebugDrawerComponent.cpp:738
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::DebugDrawerComponent::ArrowData::direction
Eigen::Vector3f direction
Definition: DebugDrawerComponent.h:675
armarx::DebugDrawerComponent::removeLayer
void removeLayer(const std::string &layerName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:2895
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::DebugDrawerComponent::select
void select(const std::string &layerName, const std::string &elementName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:183
armarx::DebugDrawerComponent::Layer::addedRobotVisualizations
std::map< std::string, SoSeparator * > addedRobotVisualizations
Definition: DebugDrawerComponent.h:814
armarx::DebugDrawerComponent::BoxData::width
float width
Definition: DebugDrawerComponent.h:604
armarx::DebugDrawerComponent::disableAllLayers
void disableAllLayers(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: DebugDrawerComponent.cpp:1750
armarx::DebugDrawerComponent::selectionNode
SoSelection * selectionNode
Definition: DebugDrawerComponent.h:837
armarx::DebugDrawerComponent::removeLine
void removeLine(const std::string &layerName, const std::string &name)
Definition: DebugDrawerComponent.cpp:1405
armarx::DebugDrawerComponent::layers
std::map< const std::string, Layer > layers
All existing layers.
Definition: DebugDrawerComponent.h:849