Visualizer.cpp
Go to the documentation of this file.
1 #include "Visualizer.h"
2 
3 #include <Eigen/Core>
4 #include <Eigen/Geometry>
5 
8 
9 #include "ExportVRML.h"
10 #include <Inventor/SoPath.h>
11 
12 namespace armarx::viz
13 {
14  namespace coin
15  {
16  void clearRobotCache();
17  void clearObjectCache();
18  } // namespace coin
19 
20  static const int ANY_TRANSFORM =
21  data::InteractionEnableFlags::TRANSLATION_X | data::InteractionEnableFlags::TRANSLATION_Y |
22  data::InteractionEnableFlags::TRANSLATION_Z | data::InteractionEnableFlags::ROTATION_X |
23  data::InteractionEnableFlags::ROTATION_Y | data::InteractionEnableFlags::ROTATION_Z |
24  data::InteractionEnableFlags::SCALING_X | data::InteractionEnableFlags::SCALING_Y |
25  data::InteractionEnableFlags::SCALING_Z;
26 
27  struct CoinVisualizerWrapper : IceUtil::Shared
28  {
30 
31  void
32  onUpdateSuccess(data::LayerUpdates const& updates)
33  {
34  this_->onUpdateSuccess(updates);
35  }
36 
37  void
38  onUpdateFailure(Ice::Exception const& ex)
39  {
41  }
42  };
43 
44  static void
45  selectionCallback(void* data, SoPath* path)
46  {
47  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
48  this_->onSelectEvent(path, data::InteractionFeedbackType::SELECT);
49  }
50 
51  static void
52  deselectionCallback(void* data, SoPath* path)
53  {
54  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
55  this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
56  }
57 
58  static void
59  startManipulationCallback(void* data, SoDragger* dragger)
60  {
61  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
62  this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
63  }
64 
65  static void
66  duringManipulationCallback(void* data, SoDragger* dragger)
67  {
68  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
69  this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
70  }
71 
72  static void
73  finishManipulationCallback(void* data, SoDragger* dragger)
74  {
75  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
76  this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
77  }
78 
79  static const char*
81  {
82  switch (state)
83  {
85  return "STOPPED";
87  return "STARTING";
89  return "RUNNING";
91  return "STOPPING";
92  }
93  return "UNKNOWN";
94  }
95 
96  struct TimedBlock
97  {
98  TimedBlock(const char* function) : start(IceUtil::Time::now()), function(function)
99  {
100  }
101 
103  {
104  IceUtil::Time diff = IceUtil::Time::now() - start;
105  // Horrible: We need to plot this actually
106  ARMARX_INFO << "Time '" << function << "': " << diff.toMilliSecondsDouble() << " ms";
107  }
108 
109  private:
110  IceUtil::Time start;
111  const char* function;
112  };
113 
115  {
117 
119  callbackData->this_ = this;
120  callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(
121  callbackData,
124 
125  manipulatorGroup = new SoSeparator;
126 
127  // The SoSelection node enable selection of nodes via mouse click / ray casting
128  selection = new SoSelection;
129  selection->addSelectionCallback(&selectionCallback, this);
130  selection->addDeselectionCallback(&deselectionCallback, this);
131  selection->setUserData(this);
132 
133  root = new SoSeparator;
134  root->addChild(manipulatorGroup);
135  root->addChild(selection);
136 
137  // Preallocate some space for layers
138  layers.data.reserve(32);
139  }
140 
142  {
143  }
144 
145  void
147  {
150  }
151 
152  void
153  CoinVisualizer::startAsync(StorageInterfacePrx const& storage)
154  {
155  std::unique_lock<std::mutex> lock(stateMutex);
157  {
158  ARMARX_WARNING << "Unexpected state of visualizer\n"
159  << "Expected: STOPPED\n"
160  << "But got: " << toString(state);
161  return;
162  }
165  }
166 
167  void
169  {
171  {
172  return;
173  }
174 
176  }
177 
179  CoinVisualizer::apply(data::LayerUpdate const& update)
180  {
181  IceUtil::Time time_start = IceUtil::Time::now();
182 
184  timing.layerName = update.name;
185 
186  CoinLayerID layerID(update.component, update.name);
187  CoinLayer& layer = findOrAddLayer(layerID);
188 
189  IceUtil::Time time_addLayer = IceUtil::Time::now();
190  timing.addLayer = time_addLayer - time_start;
191 
192  addOrUpdateElements(&layer, update);
193 
194  IceUtil::Time time_updates = IceUtil::Time::now();
195  timing.updateElements = time_updates - time_addLayer;
196 
198 
199  IceUtil::Time time_remove = IceUtil::Time::now();
200  timing.removeElements = time_remove - time_updates;
201 
202  emitLayerUpdated(layerID, layer);
203 
204  IceUtil::Time time_end = IceUtil::Time::now();
205  timing.total = time_end - time_start;
206 
207  return timing;
208  }
209 
210  CoinLayer&
212  {
213  auto layerIt = layers.lowerBound(layerID);
214 
215  if (layerIt == layers.data.end() || layerIt->id != layerID)
216  {
217  // Create a new layer
218  SoSeparator* coinNode = new SoSeparator;
219  coinNode->ref();
220  selection->addChild(coinNode);
221 
222  layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
223  layerIt->elements.reserve(64);
224  }
225 
226  return *layerIt;
227  }
228 
229  void
230  CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
231  {
232  layer->elements.reserve(update.elements.size());
233  for (viz::data::ElementPtr const& updatedElementPtr : update.elements)
234  {
235  if (!updatedElementPtr)
236  {
237  ARMARX_WARNING << deactivateSpam(10) << "Element is null in layer "
238  << update.component << "/" << update.name;
239  continue;
240  }
241  data::Element const& updatedElement = *updatedElementPtr;
242 
243  std::type_index elementType = typeid(updatedElement);
244  size_t visuIndex;
245  size_t visuSize = elementVisualizersTypes.size();
246  for (visuIndex = 0; visuIndex < visuSize; ++visuIndex)
247  {
248  if (elementVisualizersTypes[visuIndex] == elementType)
249  {
250  break;
251  }
252  }
253  if (visuIndex >= visuSize)
254  {
255  ARMARX_WARNING << deactivateSpam(1) << "No visualizer for element type found: "
256  << armarx::GetTypeString(elementType);
257  continue;
258  }
259  coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
260 
261  auto oldElementIter = layer->lowerBound(updatedElement.id);
262  CoinLayerElement* oldElement = nullptr;
263  if (oldElementIter != layer->elements.end() &&
264  oldElementIter->data->id == updatedElement.id)
265  {
266  // Element already exists
267  CoinLayerElement* oldElement = &*oldElementIter;
268  coin::ElementVisualization& oldElementVisu = *oldElement->visu;
269 
270  oldElementVisu.wasUpdated = true;
271 
272  bool updated = visualizer->update(updatedElement, &oldElementVisu);
273 
274  if (updated)
275  {
276  // Has an interaction been added?
277  viz::data::InteractionDescription& oldInteraction =
278  oldElement->data->interaction;
279  viz::data::InteractionDescription& newInteraction =
280  updatedElementPtr->interaction;
281  if (newInteraction.enableFlags != oldInteraction.enableFlags ||
282  oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
283  {
285  layer->id, updatedElement.id, newInteraction, oldElement->visu.get());
286  }
287 
288  oldElement->data = updatedElementPtr;
289  continue;
290  }
291  else
292  {
293  // Types are different, so we delete the old element and create a new one
294  layer->node->removeChild(oldElementVisu.separator);
295  }
296  }
297 
298  auto elementVisu = visualizer->create(updatedElement);
299  if (elementVisu->separator)
300  {
301  // Has the new element interactions?
302  viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
303  if (newInteraction.enableFlags != 0)
304  {
306  layer->id, updatedElement.id, newInteraction, elementVisu.get());
307  }
308 
309  layer->node->addChild(elementVisu->separator);
310  if (oldElement)
311  {
312  oldElement->data = updatedElementPtr;
313  oldElement->visu = std::move(elementVisu);
314  }
315  else
316  {
317  // Need to add a new element
318  layer->elements.insert(
319  oldElementIter,
320  CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
321  }
322  }
323  else
324  {
325  std::string typeName = armarx::GetTypeString(elementType);
326  ARMARX_WARNING << deactivateSpam(typeName, 1)
327  << "CoinElementVisualizer returned null for type: " << typeName
328  << "\n"
329  << "You need to register a visualizer for each type in "
330  "ArViz/Coin/RegisterVisualizationTypes.cpp";
331  }
332  }
333  }
334 
335  void
337  std::string const& elementID,
338  data::InteractionDescription const& interactionDesc,
340  {
341  // Lookup the interaction entry
342  ElementInteractionData* foundInteraction = nullptr;
343  for (auto& interaction : elementInteractions)
344  {
345  if (interaction->layer == layerID && interaction->element == elementID)
346  {
347  foundInteraction = interaction.get();
348  }
349  }
350  if (foundInteraction == nullptr)
351  {
352  // Need to add a new entry
353  foundInteraction = elementInteractions.emplace_back(new ElementInteractionData).get();
354  }
355  foundInteraction->layer = layerID;
356  foundInteraction->element = elementID;
357 
358  foundInteraction->interaction = interactionDesc;
359  foundInteraction->visu = visu;
360 
361  // Add user data to Coin node
362  visu->separator->setUserData(foundInteraction);
363  visu->separator->setName("InteractiveNode");
364  }
365 
366  void
368  {
369  for (auto iter = layer->elements.begin(); iter != layer->elements.end();)
370  {
371  coin::ElementVisualization& elementVisu = *iter->visu;
372  if (elementVisu.wasUpdated)
373  {
374  elementVisu.wasUpdated = false;
375  ++iter;
376  }
377  else
378  {
379  void* userData = elementVisu.separator->getUserData();
380  if (userData)
381  {
382  // Remove interaction entry if element has been removed
383  auto removedInteractionIter = std::find_if(
384  elementInteractions.begin(),
385  elementInteractions.end(),
386  [userData](std::unique_ptr<ElementInteractionData> const& entry)
387  { return entry.get() == userData; });
388  ElementInteractionData* removedInteraction = removedInteractionIter->get();
389  if (removedInteraction == selectedElement)
390  {
391  // The selected element is removed ==> Therefore deselect it
392  selection->deselectAll();
393  selectedElement = nullptr;
394  }
395  elementInteractions.erase(removedInteractionIter);
396 
397  elementVisu.separator->setUserData(nullptr);
398  elementVisu.separator->setName("");
399  }
400  layer->node->removeChild(elementVisu.separator);
401  iter = layer->elements.erase(iter);
402  }
403  }
404  }
405 
406  void
408  {
409  {
410  std::lock_guard lock(timingMutex);
412  }
413  switch (state)
414  {
416  {
417  std::unique_lock<std::mutex> lock(stateMutex);
419  selection->deselectAll();
420  selection->removeAllChildren();
421  manipulatorGroup->removeAllChildren();
423  elementInteractions.clear();
424  selectedElement = nullptr;
425  layers.data.clear();
426  pulledUpdates.revision = 0;
427  pulledUpdates.updates.clear();
430  }
431  break;
432 
434  break;
435 
437  {
438  // After we have reached the STOPPED state, we know that updates are no longer running
440  return;
441  }
442  break;
443 
445  return;
446  }
447 
448  switch (updateResult)
449  {
451  {
452  IceUtil::Time time_start = IceUtil::Time::now();
454 
455  data::LayerUpdates currentUpdates = pulledUpdates;
457  timing.waitStart = time_start;
458  storage->begin_pullUpdatesSinceAndSendInteractions(
459  currentUpdates.revision, interactionFeedbackBuffer, callback);
460 
461  // Clear interaction feedback buffer after it has been sent
463 
464  auto layerIDsBefore = getLayerIDs();
465 
466  IceUtil::Time time_pull = IceUtil::Time::now();
467  timing.pull = time_pull - time_start;
468 
469  timing.applies.reserve(currentUpdates.updates.size());
470  for (data::LayerUpdate const& update : currentUpdates.updates)
471  {
472  auto& applyTiming = timing.applies.emplace_back(apply(update));
473  timing.applyTotal.add(applyTiming);
474  }
475  IceUtil::Time time_apply = IceUtil::Time::now();
476  timing.applyTotal.total = time_apply - time_pull;
477 
478  auto layerIDsAfter = getLayerIDs();
479  if (layerIDsAfter != layerIDsBefore)
480  {
481  emitLayersChanged(layerIDsAfter);
482  }
483  IceUtil::Time time_layersChanged = IceUtil::Time::now();
484  timing.layersChanged = time_layersChanged - time_apply;
485 
486  IceUtil::Time time_end = IceUtil::Time::now();
487  timing.total = time_end - time_start;
488 
489  {
490  // Copy the timing result
491  std::lock_guard lock(timingMutex);
492  timing.counter = lastTiming.counter + 1;
494  lastTiming = std::move(timing);
495  }
496  }
497  break;
499  {
500  // Still waiting for result
501  {
502  // Copy the timing result
503  std::lock_guard lock(timingMutex);
504  lastTiming.waitDuration = IceUtil::Time::now() - lastTiming.waitStart;
505  }
506  }
507  break;
509  {
510  std::unique_lock<std::mutex> lock(stateMutex);
511  storage = nullptr;
513  }
514  break;
515  }
516  }
517 
518  void
519  CoinVisualizer::onUpdateSuccess(const data::LayerUpdates& updates)
520  {
521  pulledUpdates = updates;
523  }
524 
525  void
526  CoinVisualizer::onUpdateFailure(const Ice::Exception& ex)
527  {
528  ARMARX_WARNING << "Lost connection to ArVizStorage\n" << ex.what();
530  }
531 
532  void
533  CoinVisualizer::showLayer(CoinLayerID const& id, bool visible)
534  {
535  CoinLayer* layer = layers.findLayer(id);
536  if (layer == nullptr)
537  {
538  return;
539  }
540 
541  int childIndex = selection->findChild(layer->node);
542  if (childIndex < 0)
543  {
544  // Layer is currently not visible
545  if (visible)
546  {
547  selection->addChild(layer->node);
548  }
549  }
550  else
551  {
552  // Layer is currently visible
553  if (!visible)
554  {
555  // If we hide a layer wit a selected element, Coin will crash during rendering
556  // So we check if the selected element is in the layer to be hidden
557  if (selectedElement && selectedElement->layer == id)
558  {
559  selection->deselectAll();
560  }
561  selection->removeChild(childIndex);
562  }
563  }
564  }
565 
568  {
569  std::lock_guard lock(timingMutex);
570  return lastTiming;
571  }
572 
573  std::vector<CoinLayerID>
575  {
576  std::vector<CoinLayerID> result;
577  result.reserve(layers.data.size());
578  for (CoinLayer& layer : layers.data)
579  {
580  result.push_back(layer.id);
581  }
582  return result;
583  }
584 
585  void
586  CoinVisualizer::emitLayersChanged(std::vector<CoinLayerID> const& layerIDs)
587  {
589  {
590  layersChangedCallback(layerIDs);
591  }
592  }
593 
594  void
596  {
597  for (auto& callback : layerUpdatedCallbacks)
598  {
599  if (callback)
600  {
601  callback(layerID, layer);
602  }
603  }
604  }
605 
606  void
608  {
609  if (index >= (int)elementInteractions.size())
610  {
611  return;
612  }
613 
615 
616  selection->deselectAll();
617  selection->select(id->visu->separator);
618  }
619 
620  static ElementInteractionData*
621  findInteractionDataOnPath(SoPath* path)
622  {
623  // Search for user data in the path
624  // We stored ElementInteractionData into the user data of the parent SoSeparator
625  void* userData = nullptr;
626  int pathLength = path->getLength();
627  for (int i = 0; i < pathLength; ++i)
628  {
629  SoNode* node = path->getNode(i);
630  const char* name = node->getName().getString();
631  if (strcmp(name, "InteractiveNode") == 0)
632  {
633  userData = node->getUserData();
634  if (userData != nullptr)
635  {
636  break;
637  }
638  }
639  }
640 
641  return static_cast<ElementInteractionData*>(userData);
642  }
643 
644  void
645  CoinVisualizer::onSelectEvent(SoPath* path, int eventType)
646  {
648  {
649  return;
650  }
651 
652  ElementInteractionData* id = findInteractionDataOnPath(path);
653  if (id == nullptr)
654  {
655  if (eventType == data::InteractionFeedbackType::SELECT)
656  {
657  // An object was selected that does not have any interactions enabled
658  // We deselect all other elements in this case to avoid highlighting
659  // non-interactable elements
660  selection->deselectAll();
661  }
662  return;
663  }
664  int enableFlags = id->interaction.enableFlags;
665  if ((enableFlags & data::InteractionEnableFlags::SELECT) == 0)
666  {
667  // Element was not marked for selection
668  return;
669  }
670 
671  if (eventType == data::InteractionFeedbackType::SELECT)
672  {
673  selectedElement = id;
674 
675  // Does the element support transform interactions?
676  if (enableFlags & ANY_TRANSFORM)
677  {
678  manipulatorGroup->removeAllChildren();
679  // We need to create the manipulator everytime to achieve the desired effect
680  // If we reuse an instance, the manipulator gets stuck to old objects...
681  manipulator = new SoTransformerManip;
682  SoDragger* dragger = manipulator->getDragger();
683  dragger->addStartCallback(&startManipulationCallback, this);
684  dragger->addMotionCallback(&duringManipulationCallback, this);
685  dragger->addFinishCallback(&finishManipulationCallback, this);
686 
687  manipulatorGroup->addChild(manipulator);
688 
689  // We add the same visualization node again
690  SoSeparator* newSep = new SoSeparator();
691  int childNum = id->visu->separator->getNumChildren();
692  for (int i = 0; i < childNum; ++i)
693  {
694  SoNode* child = id->visu->separator->getChild(i);
695  if (SoSwitch* switch_ = dynamic_cast<SoSwitch*>(child))
696  {
697  child = switch_->copy();
698  }
699  newSep->addChild(child);
700  }
701  manipulatorGroup->addChild(newSep);
702  manipulator->unsquishKnobs();
703 
704  if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
705  {
706  id->visu->switch_->whichChild = SO_SWITCH_NONE;
707  }
708  }
709  else
710  {
711  selection->touch();
712  }
713  }
714  else
715  {
716  if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
717  {
718  // Now, we should apply the transformation to the original object (at least temporary)
719  // This avoids flickering, after the object is deselected
720  SbMatrix manipMatrix;
721  manipMatrix.setTransform(1000.0f * manipulator->translation.getValue(),
722  manipulator->rotation.getValue(),
723  manipulator->scaleFactor.getValue(),
724  manipulator->scaleOrientation.getValue(),
725  manipulator->center.getValue());
726  id->visu->transform->multLeft(manipMatrix);
727 
728  SbVec3f translation = id->visu->transform->translation.getValue();
729  ARMARX_IMPORTANT << "Visu translation: " << translation[0] << ", " << translation[1]
730  << ", " << translation[2];
731 
732  id->visu->switch_->whichChild = SO_SWITCH_ALL;
733  }
734 
735  selectedElement = nullptr;
736  manipulatorGroup->removeAllChildren();
737  }
738 
739  viz::data::InteractionFeedback& feedback = interactionFeedbackBuffer.emplace_back();
740  feedback.type = eventType;
741  feedback.component = id->layer.first;
742  feedback.layer = id->layer.second;
743  feedback.element = id->element;
744  feedback.revision = pulledUpdates.revision;
745  }
746 
747  SbVec3f
748  translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
749  {
750  SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
751  SbVec3f t_added_rotation_and_scale;
752  r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
753  t_added_rotation_and_scale -= t_o;
754 
755  SbVec3f t_added_rotation;
756  r_m.multVec(t_o, t_added_rotation);
757  t_added_rotation -= t_o;
758  SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
759  return t_added_scale;
760  }
761 
762  SbVec3f
763  translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
764  {
765  SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
766  SbVec3f t_added_rotation_and_scale;
767  r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
768  t_added_rotation_and_scale -= t_o;
769 
770  // Do we need to exclude
771  // SbVec3f t_added_rotation;
772  // r_m.multVec(t_o, t_added_rotation);
773  // t_added_rotation -= t_o;
774  // SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
775  return t_added_rotation_and_scale;
776  }
777 
779  toEigen(SbMat const& mat)
780  {
781  Eigen::Matrix4f result;
782  for (int y = 0; y < 4; ++y)
783  {
784  for (int x = 0; x < 4; ++x)
785  {
786  result(x, y) = mat[y][x];
787  }
788  }
789 
790  return result;
791  }
792 
793  // This should constrain the rotation according to the enabled axes
794  static SbRotation
795  constrainRotation(SbRotation input, int enableFlags)
796  {
797  SbMatrix mat;
798  mat.setRotate(input);
799  Eigen::Matrix4f mat_eigen = toEigen(mat);
800  Eigen::Matrix3f mat_rot = mat_eigen.block<3, 3>(0, 0);
801  Eigen::Vector3f rpy = mat_rot.eulerAngles(0, 1, 2);
802  // ARMARX_INFO << "rpy before: " << rpy.transpose();
803  if ((enableFlags & data::InteractionEnableFlags::ROTATION_X) == 0)
804  {
805  rpy(0) = 0.0f;
806  }
807  if ((enableFlags & data::InteractionEnableFlags::ROTATION_Y) == 0)
808  {
809  rpy(1) = 0.0f;
810  }
811  if ((enableFlags & data::InteractionEnableFlags::ROTATION_Z) == 0)
812  {
813  rpy(2) = 0.0f;
814  }
815  // ARMARX_INFO << "rpy after: " << rpy.transpose();
816 
817  mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX()) *
818  Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY()) *
819  Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
820  Eigen::Quaternionf q(mat_rot);
821 
822  SbRotation result(q.x(), q.y(), q.z(), q.w());
823  return result;
824  }
825 
826  static SbVec3f
827  constrainScaling(SbVec3f input, int enableFlags)
828  {
829  SbVec3f result = input;
830  if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
831  {
832  result[0] = 1.0f;
833  }
834  if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
835  {
836  result[1] = 1.0f;
837  }
838  if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
839  {
840  result[2] = 1.0f;
841  }
842  return result;
843  }
844 
845  void
846  CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
847  {
849  {
850  return;
851  }
852  if (selectedElement == nullptr)
853  {
854  ARMARX_WARNING << "A manipulation event was fired but no element is selected";
855  return;
856  }
857 
858  // If there is an entry in the feedback buffer already, update it
859  // This way, we prevent multiple events being sent to the client
860  viz::data::InteractionFeedback* newFeedback = nullptr;
861  for (viz::data::InteractionFeedback& feedback : interactionFeedbackBuffer)
862  {
863  if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM &&
864  feedback.component == selectedElement->layer.first &&
865  feedback.layer == selectedElement->layer.second &&
866  feedback.element == selectedElement->element)
867  {
868  // This is a transform interaction concerning the same element
869  newFeedback = &feedback;
870  newFeedback->type |= eventType;
871  }
872  }
873  if (newFeedback == nullptr)
874  {
875  // Create a new interaction
876  newFeedback = &interactionFeedbackBuffer.emplace_back();
877  newFeedback->component = selectedElement->layer.first;
878  newFeedback->layer = selectedElement->layer.second;
879  newFeedback->element = selectedElement->element;
880  newFeedback->type = data::InteractionFeedbackType::TRANSFORM | eventType;
881  }
882 
883  newFeedback->revision = pulledUpdates.revision;
884 
885 
886  // Transformation applied by the manipulator
887  SbVec3f t_m = manipulator->translation.getValue();
888  SbRotation r_m_old = manipulator->rotation.getValue();
889  SbVec3f s_m_old = manipulator->scaleFactor.getValue();
890 
891  int enableFlags = selectedElement->interaction.enableFlags;
892 
893  SbRotation r_m = constrainRotation(r_m_old, enableFlags);
894  SbVec3f s_m = constrainScaling(s_m_old, enableFlags);
895 
896  // Transformation applied to the object
897  // Translation is in mm, but the manipulator works in m!
898  SbVec3f t_o = 0.001f * selectedElement->visu->transform->translation.getValue();
899 
900  if (s_m != s_m_old)
901  {
902  manipulator->scaleFactor.setValue(s_m);
903 
904  // Remove motion induced by scaling
905  SbVec3f t_old = translationDueToScaling(r_m_old, t_m, s_m_old, t_o);
906  SbVec3f t_new = translationDueToScaling(r_m, t_m, s_m, t_o);
907  SbVec3f t_diff = t_new - t_old;
908  t_m -= t_diff;
909  }
910  if (r_m != r_m_old)
911  {
912  manipulator->rotation.setValue(r_m);
913 
914  // Remove motion induced by rotation
915  SbVec3f t_old = translationDueToRotation(r_m_old, t_m, s_m, t_o);
916  SbVec3f t_new = translationDueToRotation(r_m, t_m, s_m, t_o);
917  SbVec3f t_diff = t_new - t_old;
918  t_m -= t_diff;
919  }
920 
921  // TODO: Should we use the rotation of the object?
922  // SbRotation r_o = selectedElement->visu->transform->rotation.getValue();
923  // TODO: Do we need to consider scale of the object as well?
924  // SbVec3f s_o = selectedElement->visu->transform->scaleFactor.getValue();
925 
926  // This value stays constant during rotation and scaling!
927  // It is zero when the manipulation first starts
928  // So we can reproduce the perceived motion here (hopefully)
929  SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
930  SbVec3f t_added_rotation_and_scale;
931  r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
932  t_added_rotation_and_scale -= t_o;
933  SbVec3f delta_t = t_added_rotation_and_scale + t_m;
934 
935  SbVec3f t_added_rotation;
936  r_m.multVec(t_o, t_added_rotation);
937  t_added_rotation -= t_o;
938  SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
939 
940  // Prevent translation along disabled axes
941  if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
942  {
943  delta_t[0] = 0.0f;
944  }
945  if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Y) == 0)
946  {
947  delta_t[1] = 0.0f;
948  }
949  if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Z) == 0)
950  {
951  delta_t[2] = 0.0f;
952  }
953 
954  SbVec3f t_m_projected = delta_t - t_added_rotation_and_scale;
955  manipulator->translation.setValue(t_m_projected);
956 
957  // t_m_projected is the correct value for the manipulator, but it still contains translation due to scaling!
958  // We should subtract the translation induced by scaling!
959  SbVec3f t_m_non_scaled = t_m_projected + t_added_scale;
960 
961  data::GlobalPose& transformation = newFeedback->transformation;
962  transformation.x = 1000.0f * t_m_non_scaled[0];
963  transformation.y = 1000.0f * t_m_non_scaled[1];
964  transformation.z = 1000.0f * t_m_non_scaled[2];
965  transformation.qw = r_m[3];
966  transformation.qx = r_m[0];
967  transformation.qy = r_m[1];
968  transformation.qz = r_m[2];
969 
970  armarx::Vector3f& scale = newFeedback->scale;
971  scale.e0 = s_m[0];
972  scale.e1 = s_m[1];
973  scale.e2 = s_m[2];
974  }
975 
976  void
977  CoinVisualizer::exportToVRML(const std::string& exportFilePath)
978  {
979  coin::exportToVRML(selection, exportFilePath);
980  }
981 } // namespace armarx::viz
armarx::viz::CoinVisualizer::selectedElement
ElementInteractionData * selectedElement
Definition: Visualizer.h:260
armarx::viz::CoinLayer
Definition: Visualizer.h:34
armarx::viz::CoinVisualizer_ApplyTiming::removeElements
IceUtil::Time removeElements
Definition: Visualizer.h:162
armarx::viz::CoinVisualizer::findOrAddLayer
CoinLayer & findOrAddLayer(CoinLayerID const &layerID)
Definition: Visualizer.cpp:211
armarx::viz::coin::exportToVRML
void exportToVRML(SoNode *node, std::string const &exportFilePath)
Definition: ExportVRML.cpp:70
armarx::viz::CoinVisualizer_UpdateTiming::waitDuration
IceUtil::Time waitDuration
Definition: Visualizer.h:186
armarx::viz::CoinLayerMap::findLayer
CoinLayer * findLayer(CoinLayerID const &id)
Definition: Visualizer.h:121
armarx::viz::CoinLayer::elements
std::vector< CoinLayerElement > elements
Definition: Visualizer.h:101
armarx::viz::CoinVisualizer::pulledUpdates
data::LayerUpdates pulledUpdates
Definition: Visualizer.h:281
armarx::viz::CoinVisualizer::state
std::atomic< CoinVisualizerState > state
Definition: Visualizer.h:284
armarx::viz::CoinVisualizer::selectElement
void selectElement(int index)
Definition: Visualizer.cpp:607
armarx::viz::coin::ElementVisualization
Definition: ElementVisualizer.h:21
armarx::viz::CoinVisualizer::elementVisualizers
std::vector< std::unique_ptr< coin::ElementVisualizer > > elementVisualizers
Definition: Visualizer.h:273
armarx::viz::CoinVisualizer_UpdateTiming::total
IceUtil::Time total
Definition: Visualizer.h:183
armarx::viz::CoinVisualizer::showLayer
void showLayer(CoinLayerID const &id, bool visible)
Definition: Visualizer.cpp:533
armarx::viz::CoinVisualizer::onSelectEvent
void onSelectEvent(SoPath *path, int eventType)
Definition: Visualizer.cpp:645
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:48
armarx::viz::ElementInteractionData::visu
coin::ElementVisualization * visu
Definition: Visualizer.h:200
armarx::viz::interaction
InteractionDescription interaction()
Definition: ElementOps.h:109
GetTypeString.h
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::viz::CoinVisualizer::clearCache
void clearCache()
Definition: Visualizer.cpp:146
armarx::viz::CoinVisualizer::startAsync
void startAsync(StorageInterfacePrx const &storage)
Definition: Visualizer.cpp:153
armarx::viz::CoinVisualizer::stateStorage
viz::StorageInterfacePrx stateStorage
Definition: Visualizer.h:285
armarx::viz::CoinVisualizer::manipulator
SoTransformerManip * manipulator
Definition: Visualizer.h:278
armarx::viz::CoinVisualizer::selection
SoSelection * selection
Definition: Visualizer.h:275
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::viz::coin::clearObjectCache
void clearObjectCache()
Definition: VisualizationObject.cpp:256
armarx::viz::ElementInteractionData::layer
CoinLayerID layer
Definition: Visualizer.h:197
armarx::viz::CoinVisualizer::layerUpdatedCallbacks
std::vector< std::function< void(CoinLayerID const &layerID, CoinLayer const &layer)> > layerUpdatedCallbacks
A layer's data has changed.
Definition: Visualizer.h:291
armarx::viz::CoinVisualizer::getLayerIDs
std::vector< CoinLayerID > getLayerIDs()
Definition: Visualizer.cpp:574
armarx::viz::CoinVisualizer::onUpdateFailure
void onUpdateFailure(Ice::Exception const &ex)
Definition: Visualizer.cpp:526
armarx::viz::CoinVisualizerState::STOPPING
@ STOPPING
armarx::viz::CoinVisualizer::emitLayerUpdated
void emitLayerUpdated(CoinLayerID const &layerID, CoinLayer const &layer)
Definition: Visualizer.cpp:595
armarx::viz::CoinVisualizer::removeElementsIfNotUpdated
void removeElementsIfNotUpdated(CoinLayer *layer)
Definition: Visualizer.cpp:367
armarx::viz::coin::clearRobotCache
void clearRobotCache()
Definition: VisualizationRobot.cpp:357
armarx::viz::CoinVisualizer::stateMutex
std::mutex stateMutex
Definition: Visualizer.h:283
armarx::viz::CoinVisualizer::onManipulation
void onManipulation(SoDragger *dragger, int eventType)
Definition: Visualizer.cpp:846
armarx::viz::CoinVisualizer_UpdateTiming::pull
IceUtil::Time pull
Definition: Visualizer.h:181
armarx::viz::coin::ElementVisualizer::update
bool update(data::Element const &element, ElementVisualization *visu)
Definition: ElementVisualizer.cpp:62
armarx::viz::CoinVisualizer::callback
armarx::viz::Callback_StorageInterface_pullUpdatesSinceAndSendInteractionsPtr callback
Definition: Visualizer.h:265
armarx::viz::translationDueToScaling
SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
Definition: Visualizer.cpp:748
armarx::viz::CoinVisualizer_ApplyTiming::updateElements
IceUtil::Time updateElements
Definition: Visualizer.h:161
armarx::viz::CoinLayer::node
SoSeparator * node
Definition: Visualizer.h:100
armarx::viz::CoinVisualizerState::RUNNING
@ RUNNING
IceUtil
Definition: Instance.h:21
armarx::viz::CoinVisualizer_ApplyTiming::add
void add(CoinVisualizer_ApplyTiming const &other)
Definition: Visualizer.h:166
armarx::viz::TimedBlock::TimedBlock
TimedBlock(const char *function)
Definition: Visualizer.cpp:98
armarx::viz::coin::ElementVisualization::transform
SoTransform * transform
Definition: ElementVisualizer.h:31
ExportVRML.h
armarx::viz::CoinVisualizerState::STARTING
@ STARTING
armarx::viz::CoinVisualizer_UpdateTiming::updateToggle
int updateToggle
Definition: Visualizer.h:188
armarx::viz::CoinVisualizer_UpdateTiming::counter
int counter
Definition: Visualizer.h:190
armarx::viz::CoinVisualizerWrapper::onUpdateFailure
void onUpdateFailure(Ice::Exception const &ex)
Definition: Visualizer.cpp:38
armarx::viz::CoinVisualizer_UpdateTiming
Definition: Visualizer.h:175
armarx::viz::CoinLayer::lowerBound
auto lowerBound(std::string const &id)
Definition: Visualizer.h:76
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::viz::CoinVisualizerUpdateResult::FAILURE
@ FAILURE
armarx::viz::ElementInteractionData::interaction
viz::data::InteractionDescription interaction
Definition: Visualizer.h:199
armarx::viz::ElementInteractionData
Definition: Visualizer.h:195
armarx::viz::CoinLayer::id
CoinLayerID id
Definition: Visualizer.h:99
armarx::viz::CoinVisualizer::callbackData
IceUtil::Handle< CoinVisualizerWrapper > callbackData
Definition: Visualizer.h:264
armarx::viz::CoinVisualizer::CoinVisualizer
CoinVisualizer()
Definition: Visualizer.cpp:114
armarx::viz::CoinVisualizerUpdateResult::WAITING
@ WAITING
armarx::viz::CoinLayerMap::data
std::vector< CoinLayer > data
Definition: Visualizer.h:137
armarx::viz::CoinVisualizer_UpdateTiming::waitStart
IceUtil::Time waitStart
Definition: Visualizer.h:185
armarx::viz::translationDueToRotation
SbVec3f translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
Definition: Visualizer.cpp:763
armarx::viz::CoinVisualizer_UpdateTiming::layersChanged
IceUtil::Time layersChanged
Definition: Visualizer.h:182
armarx::viz::CoinVisualizer::registerVisualizationTypes
void registerVisualizationTypes()
Definition: RegisterVisualizationTypes.cpp:20
armarx::viz::CoinVisualizerWrapper::onUpdateSuccess
void onUpdateSuccess(data::LayerUpdates const &updates)
Definition: Visualizer.cpp:32
armarx::viz::CoinVisualizer::addOrUpdateInteraction
void addOrUpdateInteraction(CoinLayerID const &layerID, std::string const &elementID, data::InteractionDescription const &interactionDesc, coin::ElementVisualization *visu)
Definition: Visualizer.cpp:336
armarx::viz::ElementInteractionData::element
std::string element
Definition: Visualizer.h:198
armarx::viz::CoinVisualizer::~CoinVisualizer
~CoinVisualizer()
Definition: Visualizer.cpp:141
armarx::viz::CoinVisualizer::emitLayersChanged
void emitLayersChanged(std::vector< CoinLayerID > const &layerIDs)
Definition: Visualizer.cpp:586
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
armarx::viz::CoinVisualizer_ApplyTiming::total
IceUtil::Time total
Definition: Visualizer.h:163
armarx::viz::CoinVisualizer::layers
CoinLayerMap layers
Definition: Visualizer.h:270
armarx::viz::CoinLayerElement::data
data::ElementPtr data
Definition: Visualizer.h:24
armarx::viz::CoinLayerElement
Definition: Visualizer.h:22
armarx::viz::CoinVisualizer::stop
void stop()
Definition: Visualizer.cpp:168
armarx::viz::TimedBlock
Definition: Visualizer.cpp:96
armarx::viz::CoinVisualizer::exportToVRML
void exportToVRML(std::string const &exportFilePath)
Definition: Visualizer.cpp:977
armarx::viz::CoinVisualizer::addOrUpdateElements
void addOrUpdateElements(CoinLayer *layer, data::LayerUpdate const &update)
Definition: Visualizer.cpp:230
armarx::viz::CoinVisualizer::update
void update()
Definition: Visualizer.cpp:407
armarx::viz::CoinVisualizerState
CoinVisualizerState
Definition: Visualizer.h:141
armarx::viz::CoinVisualizer_ApplyTiming::layerName
std::string layerName
Definition: Visualizer.h:158
armarx::viz::CoinVisualizer_UpdateTiming::applyTotal
CoinVisualizer_ApplyTiming applyTotal
Definition: Visualizer.h:179
armarx::viz::CoinLayerElement::visu
std::unique_ptr< coin::ElementVisualization > visu
Definition: Visualizer.h:25
armarx::viz::TimedBlock::~TimedBlock
~TimedBlock()
Definition: Visualizer.cpp:102
armarx::viz::CoinVisualizerWrapper::this_
class CoinVisualizer * this_
Definition: Visualizer.cpp:29
armarx::viz::CoinVisualizer::root
SoSeparator * root
Definition: Visualizer.h:276
Visualizer.h
q
#define q
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::viz::CoinVisualizer::lastTiming
CoinVisualizer_UpdateTiming lastTiming
Definition: Visualizer.h:294
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:68
armarx::GetTypeString
std::string GetTypeString(const std::type_info &tinf, bool withoutNamespaceSpecifier=false)
Definition: GetTypeString.h:38
armarx::viz::CoinVisualizer
Definition: Visualizer.h:203
armarx::viz::CoinVisualizer::getTiming
CoinVisualizer_UpdateTiming getTiming()
Definition: Visualizer.cpp:567
armarx::viz::CoinVisualizerState::STOPPED
@ STOPPED
armarx::viz::CoinVisualizer::storage
viz::StorageInterfacePrx storage
Definition: Visualizer.h:268
armarx::viz::CoinVisualizer::apply
CoinVisualizer_ApplyTiming apply(data::LayerUpdate const &update)
Definition: Visualizer.cpp:179
armarx::viz::CoinVisualizer::onUpdateSuccess
void onUpdateSuccess(data::LayerUpdates const &updates)
Definition: Visualizer.cpp:519
armarx::viz::CoinVisualizer_UpdateTiming::applies
std::vector< CoinVisualizer_ApplyTiming > applies
Definition: Visualizer.h:177
armarx::viz::CoinVisualizer::timingMutex
std::mutex timingMutex
Definition: Visualizer.h:293
armarx::viz::CoinVisualizer_ApplyTiming
Definition: Visualizer.h:156
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::viz::CoinVisualizerWrapper
Definition: Visualizer.cpp:27
armarx::viz::CoinVisualizer::elementInteractions
std::vector< std::unique_ptr< ElementInteractionData > > elementInteractions
Definition: Visualizer.h:257
armarx::viz::coin::ElementVisualizer
Definition: ElementVisualizer.h:39
armarx::Quaternion< float, 0 >
armarx::viz::CoinVisualizer::updateResult
std::atomic< CoinVisualizerUpdateResult > updateResult
Definition: Visualizer.h:280
armarx::viz::toString
const char * toString(InteractionFeedbackType type)
Definition: Interaction.h:28
armarx::viz::coin::ElementVisualization::wasUpdated
bool wasUpdated
Definition: ElementVisualizer.h:35
armarx::viz::coin::ElementVisualizer::create
std::unique_ptr< ElementVisualization > create(data::Element const &element)
Definition: ElementVisualizer.cpp:53
armarx::viz::coin::ElementVisualization::separator
SoSeparator * separator
Definition: ElementVisualizer.h:29
armarx::viz::CoinVisualizer::layersChangedCallback
std::function< void(std::vector< CoinLayerID > const &)> layersChangedCallback
Definition: Visualizer.h:287
Logging.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::viz::CoinVisualizerUpdateResult::SUCCESS
@ SUCCESS
armarx::viz::CoinVisualizer::elementVisualizersTypes
std::vector< std::type_index > elementVisualizersTypes
Definition: Visualizer.h:272
armarx::viz::CoinLayerMap::lowerBound
auto lowerBound(CoinLayerID const &id)
Definition: Visualizer.h:114
armarx::viz
This file is part of ArmarX.
Definition: ArVizStorage.cpp:418
armarx::viz::CoinLayerID
std::pair< std::string, std::string > CoinLayerID
Definition: Visualizer.h:20
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::viz::CoinVisualizer_ApplyTiming::addLayer
IceUtil::Time addLayer
Definition: Visualizer.h:160
armarx::viz::CoinVisualizer::interactionFeedbackBuffer
std::vector< viz::data::InteractionFeedback > interactionFeedbackBuffer
Definition: Visualizer.h:258
armarx::viz::CoinVisualizer::manipulatorGroup
SoSeparator * manipulatorGroup
Definition: Visualizer.h:277