ArmarXPhysicsWorldVisualization.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-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
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "ArmarXPhysicsWorldVisualization.h"
26 
27 #include <VirtualRobot/MathTools.h>
28 #include <VirtualRobot/Nodes/RobotNodeActuator.h>
29 #include <VirtualRobot/VirtualRobotException.h>
30 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
31 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
32 #include <VirtualRobot/XML/ObjectIO.h>
33 
34 #include <ArmarXCore/core/system/ArmarXDataPath.h>
35 #include <ArmarXCore/util/CPPUtility/trace.h>
36 
37 #include <Inventor/nodes/SoUnits.h>
38 #include <MemoryX/core/GridFileManager.h>
39 #include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
40 #include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
41 
42 using namespace VirtualRobot;
43 using namespace memoryx;
44 
45 namespace armarx
46 {
47 
48  ArmarXPhysicsWorldVisualization::ArmarXPhysicsWorldVisualization()
49  {
50  //enableContactVisu = false;
51  coinVisu = new SoSeparator;
52  coinVisu->ref();
53 
54  sceneViewableSep = new SoSeparator;
55  coinVisu->addChild(sceneViewableSep);
56 
57  dynamicsVisu = new SoSeparator;
58  dynamicsVisu->ref();
59  sceneViewableSep->addChild(dynamicsVisu);
60  baseCoordVisu = new SoSeparator;
61  baseCoordVisu->ref();
62 
63  // floor is not part of the viewable visu (allows better viewAll handling)
64  floorVisu = new SoSeparator;
65  this->lastSceneData.floor = false;
66  coinVisu->addChild(floorVisu);
67 
68  modelType = VirtualRobot::SceneObject::Full;
69 
70  // initially set mutex
71  mutex.reset(new std::recursive_mutex());
72  mutexData.reset(new std::recursive_mutex());
73  }
74 
75  ArmarXPhysicsWorldVisualization::~ArmarXPhysicsWorldVisualization()
76  {
77  ARMARX_INFO << "Destroying PhysicsWorld";
78  }
79 
80  template <typename T>
81  static void
82  safeUnref(T*& p)
83  {
84  if (p)
85  {
86  p->unref();
87  p = nullptr;
88  }
89  }
90 
91  void
92  ArmarXPhysicsWorldVisualization::releaseResources()
93  {
94  ARMARX_TRACE;
95  auto l = getScopedLock();
96 
97  addedVisualizations.clear();
98  addedRobotVisualizations.clear();
99 
100  safeUnref(coinVisu);
101  safeUnref(dynamicsVisu);
102  safeUnref(baseCoordVisu);
103  }
104 
105  void
106  ArmarXPhysicsWorldVisualization::onInitComponent()
107  {
108  ARMARX_TRACE;
109  std::string top = getProperty<std::string>("ReportVisuTopicName").getValue();
110  ARMARX_VERBOSE << "Using topic " << top;
111  usingTopic(top);
112  }
113 
114  void
115  ArmarXPhysicsWorldVisualization::onConnectComponent()
116  {
117  }
118 
119  void
120  ArmarXPhysicsWorldVisualization::onDisconnectComponent()
121  {
122  }
123 
124  void
125  ArmarXPhysicsWorldVisualization::onExitComponent()
126  {
127  }
128 
129  void
130  ArmarXPhysicsWorldVisualization::reportSceneUpdated(const SceneVisuData& actualData,
131  const Ice::Current&)
132  {
133  ARMARX_TRACE;
134  auto l = getScopedDataLock();
135  this->currentSceneData = actualData;
136  }
137 
138  SoSeparator*
139  ArmarXPhysicsWorldVisualization::getVisualization()
140  {
141  return coinVisu;
142  }
143 
144  SoSeparator*
145  ArmarXPhysicsWorldVisualization::getViewableScene()
146  {
147  return sceneViewableSep;
148  }
149 
150  SoNode*
151  ArmarXPhysicsWorldVisualization::buildVisu(
152  SceneObjectPtr o,
153  VirtualRobot::SceneObject::VisualizationType visuType)
154  {
155  ARMARX_TRACE;
156  auto l = getScopedLock();
157  SoNode* n = CoinVisualizationFactory::getCoinVisualization(o, visuType);
158  n->ref();
159  return n;
160  }
161 
162  SoNode*
163  ArmarXPhysicsWorldVisualization::buildVisu(
164  RobotPtr ro,
165  VirtualRobot::SceneObject::VisualizationType visuType)
166  {
167  ARMARX_TRACE;
168  auto l = getScopedLock();
169  ARMARX_DEBUG << "buildVisu type: " << visuType;
170  std::vector<RobotNodePtr> collectedRobotNodes = ro->getRobotNodes();
171  ARMARX_DEBUG << "Robot visu with " << collectedRobotNodes.size() << " nodes";
172  SoSeparator* n = new SoSeparator();
173  n->ref();
174  for (const RobotNodePtr& node : collectedRobotNodes)
175  {
176  ARMARX_TRACE;
177  VisualizationNodePtr visu = node->getVisualization(visuType);
178  auto coinVisualizationNode = std::dynamic_pointer_cast<CoinVisualizationNode>(visu);
179  if (coinVisualizationNode)
180  {
181  ARMARX_TRACE;
182  if (coinVisualizationNode->getCoinVisualization())
183  {
184  ARMARX_TRACE;
185  ARMARX_DEBUG << "ADDING VISU " << node->getName();
186  SoNode* no = coinVisualizationNode->getCoinVisualization();
187  SoSeparator* sep = dynamic_cast<SoSeparator*>(no);
188  if (sep)
189  {
190  ARMARX_DEBUG << "ADDING SEP count " << sep->getNumChildren();
191  }
192 
193  n->addChild(no);
194  }
195  else
196  {
197  ARMARX_DEBUG << "Node " << node->getName() << " doesn't have a coin visu model";
198  }
199  }
200  else
201  {
202  ARMARX_DEBUG << "Node " << node->getName() << " doesn't have a visu model";
203  }
204  }
205 
206  ARMARX_DEBUG << "buildVisu end";
207  return n;
208  }
209 
210  void
211  ArmarXPhysicsWorldVisualization::addVisualization(
212  RobotPtr ro,
213  VirtualRobot::SceneObject::VisualizationType visuType)
214  {
215  ARMARX_TRACE;
216  auto l = getScopedLock();
217 
218  VR_ASSERT(ro);
219  removeVisualization(ro);
220 
221  if (SoNode* n = buildVisu(ro, visuType))
222  {
223  ARMARX_TRACE;
224  dynamicsVisu->addChild(n);
225  addedRobotVisualizations[ro] = n;
226  n->unref();
227  }
228 
229  // we have our own mutex protection
230  ro->setThreadsafe(false);
231  }
232 
233  void
234  ArmarXPhysicsWorldVisualization::addFloorVisualization(Eigen::Vector3f floorPos,
235  Eigen::Vector3f floorUp,
236  float floorExtendMM,
237  std::string floorTexture)
238  {
239  ARMARX_TRACE;
240  ARMARX_INFO << "Adding floor visu, texture file:" << floorTexture;
241  auto l = getScopedLock();
242  floorVisu->removeAllChildren();
243  SoNode* n = (SoNode*)CoinVisualizationFactory::CreatePlaneVisualization(
244  floorPos,
245  floorUp,
246  floorExtendMM,
247  true,
248  CoinVisualizationFactory::Color::Gray(),
249  floorTexture);
250  if (n)
251  {
252  ARMARX_TRACE;
253  floorVisu->addChild(n);
254  }
255  }
256 
257  void
258  ArmarXPhysicsWorldVisualization::removeFloorVisualization()
259  {
260  ARMARX_TRACE;
261  auto l = getScopedLock();
262  floorVisu->removeAllChildren();
263  }
264 
265  void
266  ArmarXPhysicsWorldVisualization::showBaseCoord(bool enable, float scale)
267  {
268  ARMARX_TRACE;
269  auto l = getScopedLock();
270 
271  if (enable && sceneViewableSep->findChild(baseCoordVisu) < 0)
272  {
273  ARMARX_TRACE;
274  baseCoordVisu->removeAllChildren();
275  std::string str("Root");
276  baseCoordVisu->addChild(
277  VirtualRobot::CoinVisualizationFactory::CreateCoordSystemVisualization(scale,
278  &str));
279  sceneViewableSep->addChild(baseCoordVisu);
280  }
281 
282  if (!enable && sceneViewableSep->findChild(baseCoordVisu) >= 0)
283  {
284  ARMARX_TRACE;
285  sceneViewableSep->removeChild(baseCoordVisu);
286  }
287  }
288 
289  void
290  ArmarXPhysicsWorldVisualization::addVisualization(
291  SceneObjectPtr so,
292  VirtualRobot::SceneObject::VisualizationType visuType)
293  {
294  ARMARX_TRACE;
295  ARMARX_CHECK_NOT_NULL(so);
296 
297  auto l = getScopedLock();
298  removeVisualization(so);
299  SoNode* n = buildVisu(so, visuType);
300  ARMARX_DEBUG << "addVisualization " << so->getName() << " Ref: " << n->getRefCount();
301  if (n)
302  {
303  ARMARX_TRACE;
304  dynamicsVisu->addChild(n);
305  addedVisualizations[so] = n;
306  n->unref();
307  ARMARX_DEBUG << "addVisualization " << so->getName() << " Ref: " << n->getRefCount();
308  }
309  }
310 
311  void
312  ArmarXPhysicsWorldVisualization::removeVisualization(SceneObjectPtr so)
313  {
314  ARMARX_TRACE;
315  auto l = getScopedLock();
316  ARMARX_DEBUG << "removeVisualization" << flush;
317  VR_ASSERT(so);
318 
319  if (addedVisualizations.find(so) != addedVisualizations.end())
320  {
321  ARMARX_TRACE;
322  SoNode* node = addedVisualizations[so];
323  ARMARX_DEBUG << "RemoveVisualization: " << so->getName()
324  << ", Ref: " << node->getRefCount();
325  addedVisualizations.erase(so);
326  dynamicsVisu->removeChild(node);
327  }
328  }
329 
330  void
331  ArmarXPhysicsWorldVisualization::removeObjectVisualization(const std::string& name)
332  {
333  ARMARX_TRACE;
334  auto l = getScopedLock();
335  ARMARX_DEBUG << "removeVisualization";
336 
337  // removeVisualizations erases elements from addedVisualizations so we have to make a copy
338  auto copiedVisus = addedVisualizations;
339  for (auto& o : copiedVisus)
340  {
341  ARMARX_TRACE;
342  VirtualRobot::SceneObjectPtr const& so = o.first;
343  if (so->getName() == name)
344  {
345  ARMARX_TRACE;
346  removeVisualization(so);
347  return;
348  }
349  }
350 
351  ARMARX_ERROR << "No object with name " << name << " found";
352  }
353 
354  void
355  ArmarXPhysicsWorldVisualization::removeRobotVisualization(const std::string& name)
356  {
357  ARMARX_TRACE;
358  auto l = getScopedLock();
359  ARMARX_DEBUG << "remove robot visualization:" << name;
360  VirtualRobot::RobotPtr so;
361 
362  for (auto& r : addedRobotVisualizations)
363  {
364  ARMARX_TRACE;
365  if (r.first->getName() == name)
366  {
367  ARMARX_TRACE;
368  removeVisualization(r.first);
369  return;
370  }
371  }
372 
373  ARMARX_ERROR << "No robot with name " << name << " found";
374  }
375 
376  void
377  ArmarXPhysicsWorldVisualization::removeVisualization(RobotPtr r)
378  {
379  ARMARX_TRACE;
380  auto l = getScopedLock();
381  ARMARX_DEBUG << "removeVisualization robot";
382  VR_ASSERT(r);
383 
384  if (addedRobotVisualizations.find(r) != addedRobotVisualizations.end())
385  {
386  ARMARX_TRACE;
387  ARMARX_DEBUG << "found added robot, removing " << r->getName();
388  SoNode* node = addedRobotVisualizations[r];
389  dynamicsVisu->removeChild(node);
390  addedRobotVisualizations.erase(r);
391  }
392  ARMARX_DEBUG << "removeVisualization robot end";
393  }
394 
395  void
396  ArmarXPhysicsWorldVisualization::enableVisuType(bool fullModel)
397  {
398  ARMARX_TRACE;
399  auto l = getScopedLock();
400  ARMARX_DEBUG << "enableVisuType" << flush;
401 
402  if (fullModel && modelType == VirtualRobot::SceneObject::Full)
403  {
404  return;
405  }
406 
407  if (!fullModel && modelType == VirtualRobot::SceneObject::Collision)
408  {
409  return;
410  }
411  ARMARX_TRACE;
412 
413  if (fullModel)
414  {
415  modelType = VirtualRobot::SceneObject::Full;
416  }
417  else
418  {
419  modelType = VirtualRobot::SceneObject::Collision;
420  }
421 
422  if (fullModel)
423  {
424  ARMARX_INFO << "Switching to visuType full:";
425  }
426  else
427  {
428  ARMARX_INFO << "Switching to visuType colModel:";
429  }
430 
431  dynamicsVisu->removeAllChildren();
432 
433  // rebuild robots
434  for (const auto& pair : addedRobotVisualizations)
435  {
436  ARMARX_TRACE;
437  const RobotPtr& ro = pair.first;
438  SoNode* n = buildVisu(ro, modelType);
439 
440  if (n)
441  {
442  dynamicsVisu->addChild(n);
443  addedRobotVisualizations[ro] = n;
444  n->unref();
445  }
446  else
447  {
448  ARMARX_DEBUG << "Robot " << ro->getName() << " has no visualization for model type "
449  << modelType;
450  }
451  }
452  ARMARX_TRACE;
453 
454  // rebuild objects
455  std::map<VirtualRobot::SceneObjectPtr, SoNode*>::iterator it2 = addedVisualizations.begin();
456 
457  for (; it2 != addedVisualizations.end(); it2++)
458  {
459  ARMARX_TRACE;
460  SceneObjectPtr so = it2->first;
461  SoNode* n = buildVisu(so, modelType);
462 
463  if (n)
464  {
465  dynamicsVisu->addChild(n);
466  addedVisualizations[so] = n;
467  n->unref();
468  }
469  }
470  }
471 
472  void
473  ArmarXPhysicsWorldVisualization::setMutex(std::shared_ptr<std::recursive_mutex> const& m)
474  {
475  mutex = m;
476  }
477 
478  auto
479  ArmarXPhysicsWorldVisualization::getScopedLock() -> RecursiveLockPtr
480  {
481  RecursiveLockPtr l(new std::unique_lock(*mutex));
482  return l;
483  }
484 
485  auto
486  ArmarXPhysicsWorldVisualization::getScopedDataLock() -> RecursiveLockPtr
487  {
488  RecursiveLockPtr l(new std::unique_lock(*mutexData));
489  return l;
490  }
491 
492  /*
493  void ArmarXPhysicsWorldVisualization::enableContactVisu(bool enable)
494  {
495  contactVisuEnabled = enable;
496  }*/
497 
498 
499  bool
500  ArmarXPhysicsWorldVisualization::synchronizeSceneObjectPoses(NamePoseMap& objMap,
501  NameValueMap& jvMap,
502  SceneObjectPtr currentObjVisu)
503  {
504  ARMARX_TRACE;
505  if (!currentObjVisu)
506  {
507  return false;
508  }
509 
510 #if 0
511 
512  // some sanity checks:
513  if (objMap.find(currentObjVisu->getName()) == objMap.end())
514  {
515  ARMARX_ERROR << "No object with name " << currentObjVisu->getName() << "in objMap";
516  }
517  else
518  {
519  ARMARX_IMPORTANT << "object " << currentObjVisu->getName() << ":\n" << objMap[currentObjVisu->getName()] ;
520  }
521 
522 #endif
523 
524  if (objMap.find(currentObjVisu->getName()) != objMap.end())
525  {
526  ARMARX_TRACE;
527  VirtualRobot::RobotNodePtr rnv =
528  std::dynamic_pointer_cast<VirtualRobot::RobotNode>(currentObjVisu);
529  PosePtr p = PosePtr::dynamicCast(objMap[currentObjVisu->getName()]);
530  Eigen::Matrix4f gp = p->toEigen();
531 
532  if (rnv)
533  {
534  ARMARX_TRACE;
535  // we can update the joint value via an RobotNodeActuator
536  RobotNodeActuatorPtr rna(new RobotNodeActuator(rnv));
537  rna->updateVisualizationPose(gp, jvMap[currentObjVisu->getName()], false);
538  }
539  else
540  {
541  ARMARX_TRACE;
542  currentObjVisu->setGlobalPoseNoChecks(gp);
543  }
544  }
545  ARMARX_TRACE;
546  std::vector<SceneObjectPtr> childrenV = currentObjVisu->getChildren();
547 
548  for (const auto& i : childrenV)
549  {
550  ARMARX_TRACE;
551  if (!synchronizeSceneObjectPoses(objMap, jvMap, i))
552  {
553  return false;
554  }
555  }
556 
557  return true;
558  }
559 
560  bool
561  ArmarXPhysicsWorldVisualization::synchronizeFloor()
562  {
563  ARMARX_TRACE;
564  if (lastSceneData.floor == currentSceneData.floor &&
565  lastSceneData.floorTextureFile == currentSceneData.floorTextureFile)
566  {
567  return true;
568  }
569 
570  if (currentSceneData.floor)
571  {
572  ARMARX_TRACE;
573  VirtualRobot::MathTools::Plane p = VirtualRobot::MathTools::getFloorPlane();
574  addFloorVisualization(p.p, p.n, 50000.0f, currentSceneData.floorTextureFile);
575  }
576  else
577  {
578  removeFloorVisualization();
579  }
580 
581  return true;
582  }
583 
584  bool
585  ArmarXPhysicsWorldVisualization::synchronizeRobots()
586  {
587  ARMARX_TRACE;
588  for (auto& robotDef : currentSceneData.robots)
589  {
590  ARMARX_TRACE;
591  if (!robotDef.updated)
592  {
593  continue;
594  }
595 
596  // check if we need to load the robot
597  bool robotLoaded = false;
598 
599  for (auto& currentRob : lastSceneData.robots)
600  {
601  ARMARX_TRACE;
602  if (robotDef.name == currentRob.name)
603  {
604  robotLoaded = true;
605  break;
606  }
607  }
608 
609  if (!robotLoaded)
610  {
611  ARMARX_TRACE;
612  ARMARX_INFO << deactivateSpam() << "New robot:" << robotDef.name;
613 
614  if (!loadRobot(robotDef.name,
615  robotDef.robotFile,
616  robotDef.project,
617  robotDef.scaling,
618  robotDef.colModel))
619  {
620  ARMARX_ERROR << deactivateSpam() << "Loading robot failed";
621  continue;
622  }
623  }
624 
625  ARMARX_TRACE;
626  VirtualRobot::RobotPtr rob = getRobot(robotDef.name);
627 
628  if (!rob)
629  {
630  ARMARX_ERROR << "internal error, no robot with name " << robotDef.name;
631 
632  for (auto& r : addedRobotVisualizations)
633  {
634  ARMARX_ERROR << "Available Robot:" << r.first->getName();
635  }
636 
637  continue;
638  }
639  ARMARX_TRACE;
640 
641  VirtualRobot::SceneObjectPtr currentObjVisu = rob->getRootNode();
642 
643  if (!synchronizeSceneObjectPoses(
644  robotDef.robotNodePoses, robotDef.jointValues, currentObjVisu))
645  {
646  ARMARX_ERROR << deactivateSpam() << "Failed to synchronize objects...";
647  continue;
648  }
649  ARMARX_TRACE;
650 
651  // Run forward kinematics, including propagating joint values.
652  const bool updateChildren = true;
653  currentObjVisu->updatePose(updateChildren);
654 
655  // never use this data again
656  robotDef.updated = false;
657  }
658  ARMARX_TRACE;
659 
660  // check if a robot is removed
661  for (auto& currentRob : lastSceneData.robots)
662  {
663  ARMARX_TRACE;
664  bool robFound = false;
665 
666  for (auto& robotDef : currentSceneData.robots)
667  {
668  if (robotDef.name == currentRob.name)
669  {
670  robFound = true;
671  break;
672  }
673  }
674 
675  if (!robFound)
676  {
677  ARMARX_TRACE;
678  ARMARX_INFO << "Removing robot:" << currentRob.name;
679  removeRobotVisualization(currentRob.name);
680  }
681  }
682 
683  return true;
684  }
685 
686  bool
687  ArmarXPhysicsWorldVisualization::synchronizeObjects()
688  {
689  ARMARX_TRACE;
690  for (auto& objectDef : currentSceneData.objects)
691  {
692  ARMARX_TRACE;
693  if (!objectDef.updated)
694  {
695  continue;
696  }
697 
698  // check if we need to load the robot
699  bool objectLoaded = false;
700 
701  for (auto& currentObj : lastSceneData.objects)
702  {
703  if (objectDef.name == currentObj.name && getObject(objectDef.name))
704  {
705  objectLoaded = true;
706  break;
707  }
708  }
709  ARMARX_TRACE;
710 
711  if (!objectLoaded)
712  {
713  ARMARX_INFO << deactivateSpam() << "New Object:" << objectDef.name;
714 
715  if (!loadObject(objectDef))
716  {
717  ARMARX_ERROR << deactivateSpam(5, objectDef.name) << "Loading object "
718  << objectDef.name << " failed"
719  << "\n " << VAROUT(objectDef.name) << "\n "
720  << VAROUT(objectDef.objectClassName) << "\n "
721  << VAROUT(objectDef.filename) << "\n "
722  << VAROUT(objectDef.project);
723  continue;
724  }
725  }
726  ARMARX_TRACE;
727 
728  VirtualRobot::SceneObjectPtr ob = getObject(objectDef.name);
729  if (!ob)
730  {
731  ARMARX_ERROR << "internal error, no object for " << objectDef.name;
732  continue;
733  }
734 
735  NameValueMap tmpMap;
736 
737  if (!synchronizeSceneObjectPoses(objectDef.objectPoses, tmpMap, ob))
738  {
739  ARMARX_ERROR << deactivateSpam() << "Failed to synchronize objects...";
740  continue;
741  }
742  ARMARX_TRACE;
743 
744  // never use this data again
745  objectDef.updated = false;
746  }
747  ARMARX_TRACE;
748 
749  // check if a object is removed
750  for (auto& currentObj : lastSceneData.objects)
751  {
752  ARMARX_TRACE;
753  bool objFound = false;
754 
755  for (auto& objectDef : currentSceneData.objects)
756  {
757  if (objectDef.name == currentObj.name)
758  {
759  objFound = true;
760  break;
761  }
762  }
763  ARMARX_TRACE;
764 
765  if (!objFound)
766  {
767  ARMARX_INFO << "Removing object:" << currentObj.name;
768  removeObjectVisualization(currentObj.name);
769  }
770  }
771 
772  return true;
773  }
774 
775  bool
776  ArmarXPhysicsWorldVisualization::synchronizeVisualizationData()
777  {
778  ARMARX_TRACE;
779  IceUtil::Time startTime = IceUtil::Time::now();
780 
781  auto lockVisu = getScopedLock();
782  auto lockData = getScopedDataLock();
783 
784  IceUtil::Time durationlock = IceUtil::Time::now() - startTime;
785 
786  if (durationlock.toMilliSecondsDouble() > 10)
787  {
788  ARMARX_INFO << deactivateSpam(10)
789  << "Copy and Lock took long: " << durationlock.toMilliSecondsDouble()
790  << " ms";
791  }
792  ARMARX_TRACE;
793 
794 
795  // check for updates
796  auto startTime1 = IceUtil::Time::now();
797 
798  synchronizeFloor();
799 
800  IceUtil::Time duration1 = IceUtil::Time::now() - startTime1;
801  if (duration1.toMilliSecondsDouble() > 10)
802  {
803  ARMARX_INFO << deactivateSpam(10)
804  << " floorSync took long: " << duration1.toMilliSecondsDouble() << " ms";
805  }
806  ARMARX_TRACE;
807 
808  auto startTime2 = IceUtil::Time::now();
809 
810  synchronizeRobots();
811 
812  IceUtil::Time duration2 = IceUtil::Time::now() - startTime2;
813  if (duration2.toMilliSecondsDouble() > 10)
814  {
815  ARMARX_INFO << deactivateSpam(10)
816  << " RobotSync took long: " << duration2.toMilliSecondsDouble() << " ms";
817  }
818 
819 
820  auto startTime3 = IceUtil::Time::now();
821 
822  synchronizeObjects();
823 
824  IceUtil::Time duration3 = IceUtil::Time::now() - startTime3;
825  if (duration3.toMilliSecondsDouble() > 10)
826  {
827  ARMARX_INFO << deactivateSpam(10)
828  << " ObjectSync took long: " << duration3.toMilliSecondsDouble() << " ms";
829  }
830  ARMARX_TRACE;
831 
832  this->lastSceneData = this->currentSceneData;
833 
834  IceUtil::Time duration = IceUtil::Time::now() - startTime;
835  synchronize2TimeMS = (float)duration.toMilliSecondsDouble();
836 
837  return true;
838  }
839 
840  float
841  ArmarXPhysicsWorldVisualization::getSyncVisuTime()
842  {
843  return synchronize2TimeMS;
844  }
845 
846  bool
847  ArmarXPhysicsWorldVisualization::loadObject(const ObjectVisuData& obj)
848  {
849  ARMARX_TRACE;
850 
851  // try loading from file
852  if (not(obj.filename.empty() or obj.project.empty()))
853  {
854  ARMARX_TRACE;
855  return loadObject(obj.filename, obj.project, obj.name);
856  }
857  ARMARX_TRACE;
858 
859  // try loading from memory => nope. outdated
860 
861  ARMARX_TRACE;
862 
863  // try to ceate primitive
864  return createPrimitiveObject(obj.objectPrimitiveData, obj.name);
865  }
866 
867  bool
868  ArmarXPhysicsWorldVisualization::loadRobot(const std::string& robotName,
869  const std::string& robotFile,
870  const std::string& project,
871  float scaling,
872  bool colModel)
873  {
874  ARMARX_TRACE;
875  if (robotFile.empty())
876  {
877  ARMARX_INFO << deactivateSpam() << "Empty file";
878  return false;
879  }
880 
881  if (!project.empty())
882  {
883  ARMARX_TRACE;
884  ARMARX_INFO << "Adding to datapaths of " << project;
885  armarx::CMakePackageFinder finder(project);
886 
887  if (!finder.packageFound())
888  {
889  ARMARX_ERROR << "ArmarX Package " << project << " has not been found!";
890  }
891  else
892  {
893  ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
894  armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
895  }
896  }
897  ARMARX_TRACE;
898 
899  // load robot
900  std::string fn = robotFile;
901  ArmarXDataPath::getAbsolutePath(fn, fn);
902  ARMARX_INFO << "Loading robot from " << fn;
903  VirtualRobot::RobotPtr result;
904 
905  try
906  {
907  ARMARX_TRACE;
908  VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
909 
910  if (colModel)
911  {
912  loadMode = VirtualRobot::RobotIO::eCollisionModel;
913  }
914  result = RobotIO::loadRobot(fn, loadMode);
915  ARMARX_DEBUG << "Loading robot done ";
916  }
917  catch (...)
918  {
919  ARMARX_IMPORTANT << "Robot loading failed, file:" << fn;
920  return false;
921  }
922  ARMARX_TRACE;
923 
924  if (scaling != 1.0f)
925  {
926  ARMARX_INFO << "Scaling robot, factor: " << scaling;
927  result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
928  }
929 
930 
931  // ensure consistent names
932  ARMARX_DEBUG << "Setting up newly loaded robot";
933  result->setName(robotName);
934  result->setPropagatingJointValuesEnabled(true);
935  ARMARX_DEBUG << "adding newly loaded robot to visualization";
936  VirtualRobot::SceneObject::VisualizationType visuMode = modelType;
937  if (colModel)
938  {
939  ARMARX_TRACE;
940  visuMode = VirtualRobot::SceneObject::Collision;
941  }
942  addVisualization(result, visuMode);
943  return true;
944  }
945 
946  bool
947  ArmarXPhysicsWorldVisualization::loadObject(const std::string& objectFile,
948  const std::string& project,
949  const std::string& instanceName)
950  {
951  ARMARX_TRACE;
952  if (objectFile.empty())
953  {
954  ARMARX_INFO << deactivateSpam() << "Empty file";
955  return false;
956  }
957 
958  if (!project.empty())
959  {
960  ARMARX_TRACE;
961  ARMARX_INFO << "Adding to datapaths of " << project;
962  armarx::CMakePackageFinder finder(project);
963 
964  if (!finder.packageFound())
965  {
966  ARMARX_ERROR << "ArmarX Package " << project << " has not been found!";
967  }
968  else
969  {
970  ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
971  armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
972  }
973  }
974  ARMARX_TRACE;
975 
976  // load object
977  std::string fn = objectFile;
978  ArmarXDataPath::getAbsolutePath(fn, fn);
979  ARMARX_INFO << "Loading object from " << fn;
980 
981  ObstaclePtr o;
982  if (!o)
983  {
984  try
985  {
986  ARMARX_TRACE;
987  o = ObjectIO::loadManipulationObject(fn);
988  }
989  catch (VirtualRobotException& e)
990  {
991  ARMARX_ERROR << " ERROR while creating manip (file:" << fn << ")\n" << e.what();
992  return false;
993  }
994  }
995  if (!o)
996  {
997  try
998  {
999  ARMARX_TRACE;
1000  o = ObjectIO::loadObstacle(fn);
1001  }
1002  catch (VirtualRobotException& e)
1003  {
1004  //ARMARX_ERROR_S << " ERROR while creating obstacle (file:" << fn << ")" << endl << e.what() ;
1005  //return false;
1006  }
1007  }
1008  if (!o)
1009  {
1010  ARMARX_ERROR << " ERROR while creating (file:" << fn << ")";
1011  return false;
1012  }
1013 
1014  o->setName(instanceName);
1015  addVisualization(o, modelType);
1016  return true;
1017  }
1018 
1019  SceneObjectPtr
1020  ArmarXPhysicsWorldVisualization::getObject(const std::string& name)
1021  {
1022  ARMARX_TRACE;
1023  for (auto& [object, visu] : addedVisualizations)
1024  {
1025  if (object->getName() == name)
1026  {
1027  return object;
1028  }
1029  }
1030  return nullptr;
1031  }
1032 
1033  std::vector<RobotPtr>
1034  ArmarXPhysicsWorldVisualization::getRobots() const
1035  {
1036  ARMARX_TRACE;
1037  std::vector<RobotPtr> result;
1038  result.reserve(addedRobotVisualizations.size());
1039  for (auto& r : addedRobotVisualizations)
1040  {
1041  result.push_back(r.first);
1042  }
1043  return result;
1044  }
1045 
1046  std::vector<SceneObjectPtr>
1047  ArmarXPhysicsWorldVisualization::getObjects() const
1048  {
1049  ARMARX_TRACE;
1050  std::vector<SceneObjectPtr> result;
1051  result.reserve(addedVisualizations.size());
1052  for (auto& r : addedVisualizations)
1053  {
1054  result.push_back(r.first);
1055  }
1056  return result;
1057  }
1058 
1059  Ice::Long
1060  ArmarXPhysicsWorldVisualization::getSyncTimestamp() const
1061  {
1062  // currentSceneData.timestamp seems to use milliseconds
1063  return currentSceneData.timestamp * 1000;
1064  }
1065 
1066  bool
1067  ArmarXPhysicsWorldVisualization::createPrimitiveObject(
1068  ObjectVisuPrimitivePtr objectPrimitiveData,
1069  const std::string& name)
1070  {
1071  ARMARX_TRACE;
1072  if (!objectPrimitiveData)
1073  {
1074  ARMARX_INFO << deactivateSpam() << "Empty primitive data";
1075  return false;
1076  }
1077 
1078  VirtualRobot::ObstaclePtr o;
1079 
1080  // create object
1081  switch (objectPrimitiveData->type)
1082  {
1083  case Box:
1084  {
1085  ARMARX_TRACE;
1086  BoxVisuPrimitivePtr box = BoxVisuPrimitivePtr::dynamicCast(objectPrimitiveData);
1087 
1088  if (!box)
1089  {
1090  ARMARX_ERROR << "Could not cast object...";
1091  return false;
1092  }
1093 
1094  VirtualRobot::VisualizationFactory::Color c;
1095  c.r = box->color.r;
1096  c.g = box->color.g;
1097  c.b = box->color.b;
1098  c.transparency = box->color.a;
1099  o = VirtualRobot::Obstacle::createBox(box->width, box->height, box->depth, c);
1100  o->setMass(box->massKG);
1101  }
1102  break;
1103 
1104  case Sphere:
1105  {
1106  ARMARX_TRACE;
1107  SphereVisuPrimitivePtr s = SphereVisuPrimitivePtr::dynamicCast(objectPrimitiveData);
1108 
1109  if (!s)
1110  {
1111  ARMARX_ERROR << "Could not cast object...";
1112  return false;
1113  }
1114 
1115  VirtualRobot::VisualizationFactory::Color c;
1116  c.r = s->color.r;
1117  c.g = s->color.g;
1118  c.b = s->color.b;
1119  c.transparency = s->color.a;
1120  o = VirtualRobot::Obstacle::createSphere(s->radius, c);
1121  o->setMass(s->massKG);
1122  }
1123  break;
1124 
1125  case Cylinder:
1126  {
1127  ARMARX_TRACE;
1128  CylinderVisuPrimitivePtr s =
1129  CylinderVisuPrimitivePtr::dynamicCast(objectPrimitiveData);
1130 
1131  if (!s)
1132  {
1133  ARMARX_ERROR << "Could not cast object...";
1134  return false;
1135  }
1136 
1137  VirtualRobot::VisualizationFactory::Color c;
1138  c.r = s->color.r;
1139  c.g = s->color.g;
1140  c.b = s->color.b;
1141  c.transparency = s->color.a;
1142  o = VirtualRobot::Obstacle::createCylinder(s->radius, s->length, c);
1143  o->setMass(s->massKG);
1144  }
1145  break;
1146 
1147  default:
1148  ARMARX_ERROR << "Object type nyi...";
1149  return false;
1150  }
1151  ARMARX_TRACE;
1152 
1153  if (!o)
1154  {
1155  ARMARX_ERROR << " ERROR while creating object";
1156  return false;
1157  }
1158 
1159  o->setName(name);
1160  addVisualization(o, modelType);
1161  return true;
1162  }
1163 
1164  RobotPtr
1165  ArmarXPhysicsWorldVisualization::getRobot(const std::string& name)
1166  {
1167  ARMARX_TRACE;
1168  for (auto& r : addedRobotVisualizations)
1169  {
1170  if (r.first->getName() == name)
1171  {
1172  return r.first;
1173  }
1174  }
1175 
1176  return RobotPtr();
1177  }
1178 
1179 
1180 } // namespace armarx
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:43
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::CMakePackageFinder::packageFound
bool packageFound() const
Returns whether or not this package was found with cmake.
Definition: CMakePackageFinder.cpp:511
VirtualRobot
Definition: FramedPose.h:42
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
trace.h
GridFileManager.h
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
so
linux gnu libIceDB so
Definition: CMakeLists.txt:7
ObjectClass.h
project
std::string project
Definition: VisualizationRobot.cpp:85
IceInternal::Handle< Pose >
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
Color
uint32_t Color
RGBA color.
Definition: color.h:8
ArmarXPhysicsWorldVisualization.h
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:194
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:918
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::ArmarXPhysicsWorldVisualization::RecursiveLockPtr
std::shared_ptr< std::unique_lock< std::recursive_mutex > > RecursiveLockPtr
Definition: ArmarXPhysicsWorldVisualization.h:163
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:88
SimoxObjectWrapper.h
memoryx::Box
Definition: Box.hpp:32
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
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
Sphere
Definition: Sphere.h:27
Cylinder
Definition: Cylinder.h:22
T
float T
Definition: UnscentedKalmanFilterTest.cpp:38
ArmarXDataPath.h
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19