CollisionCheckerComponent.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotComponents::ArmarXObjects::CollisionCheckerComponent
17  * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <stdlib.h>
26 #include <cmath>
27 #include <sstream>
28 #include <iterator>
29 
32 
39 
41 
42 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
43 
44 #include <SimoxUtility/algorithm/string/string_tools.h>
45 
46 #include <boost/regex.hpp>
47 
48 using namespace armarx;
49 
51  connected(false)
52 {
53 
54 }
55 
57 {
58  interval = getProperty<int>("interval").getValue();
59 
60  useWorkingMemory = getProperty<bool>("UseWorkingMemory").getValue();
61  if (useWorkingMemory)
62  {
63  workingMemoryName = getProperty<std::string>("WorkingMemoryName").getValue();
64  ARMARX_INFO << "Using WorkingMemory \"" << workingMemoryName << "\" to get SceneObjects." << std::endl;
65  usingProxy(workingMemoryName);
66  usingTopic(getProperty<std::string>("WorkingMemoryListenerTopicName").getValue());
67  }
68  else
69  {
70  robotStateComponentName = getProperty<std::string>("RobotStateComponentName").getValue();
71  ARMARX_INFO << "Using RobotStateComponent \"" << robotStateComponentName << "\" to get SceneObjects." << std::endl;
72  usingProxy(robotStateComponentName);
73  }
74 
75  std::string collisionPairsString = getProperty<std::string>("CollisionPairs").getValue();
76  boost::regex regexCollisionPair("[\\[\\{\\(](([^:,\\[\\{\\(\\]\\}\\)]+:)?([^,\\[\\{\\(\\]\\}\\)]+,|[\\[\\{\\(]([^,\\[\\{\\(\\]\\}\\)]+,)*[^,\\[\\{\\(\\]\\}\\)]+[\\]\\}\\)],)){2}[0-9]*[.]?[0-9]*[\\]\\}\\)]");
77 
78  boost::sregex_token_iterator iterCollisionPair(collisionPairsString.begin(), collisionPairsString.end(), regexCollisionPair, 0);
79  boost::sregex_token_iterator end;
80 
81  for (; iterCollisionPair != end; ++iterCollisionPair)
82  {
83  std::string collisionPairString = *iterCollisionPair;
84  while ((collisionPairString[0] == '{' && collisionPairString[collisionPairString.size() - 1] == '}') ||
85  (collisionPairString[0] == '(' && collisionPairString[collisionPairString.size() - 1] == ')') ||
86  (collisionPairString[0] == '[' && collisionPairString[collisionPairString.size() - 1] == ']'))
87  {
88  collisionPairString = collisionPairString.substr(1, collisionPairString.size() - 2);
89  }
90 
91  if ((collisionPairString[0] == '{' || collisionPairString[0] == '(' || collisionPairString[0] == '[') &&
92  (collisionPairString[collisionPairString.size() - 1] == '}' || collisionPairString[collisionPairString.size() - 1] == ')' || collisionPairString[collisionPairString.size() - 1] == ']'))
93  {
94  ARMARX_ERROR << "Could not parse collision pair: " << *iterCollisionPair << ". Ignoring..." << std::endl;
95  continue;
96  }
97 
98  boost::regex regex("([^:,\\[\\{\\(\\]\\}\\)]+:)?([\\{\\[\\(][^\\{\\[\\(\\]\\}\\)]+[\\]\\}\\)]|[^\\{\\[\\(\\]\\}\\),]+)");
99 
100  boost::sregex_token_iterator iter(collisionPairString.begin(), collisionPairString.end(), regex, 0);
101  boost::sregex_token_iterator end;
102 
103  ARMARX_CHECK_EXPRESSION(iter != end);
104 
105  std::string robotName1;
106  std::vector<std::string> nodeNames1;
107  bool usesNodeSet1;
108  std::string nodeSetName1;
109  parseNodeSet(*iter, robotName1, nodeNames1, usesNodeSet1, nodeSetName1);
110 
111  ++iter;
112  ARMARX_CHECK_EXPRESSION(iter != end);
113 
114  std::string robotName2;
115  std::vector<std::string> nodeNames2;
116  bool usesNodeSet2;
117  std::string nodeSetName2;
118  parseNodeSet(*iter, robotName2, nodeNames2, usesNodeSet2, nodeSetName2);
119 
120  ++iter;
121  ARMARX_CHECK_EXPRESSION(iter != end);
122 
123  addCollisionPair(robotName1, nodeNames1, usesNodeSet1, nodeSetName1, robotName2, nodeNames2, usesNodeSet2, nodeSetName2, std::atof(((std::string)*iter).c_str()));
124 
125  ARMARX_CHECK_EXPRESSION(++iter == end);
126  }
127 
128  collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
129 
130  offeringTopic(getProperty<std::string>("CollisionListenerTopicName").getValue());
131  offeringTopic(getProperty<std::string>("DistanceListenerTopicName").getValue());
132 
133  useDebugDrawer = getProperty<bool>("UseDebugDrawer").getValue();
134 }
135 
136 
138 {
139  {
140  std::scoped_lock lockData(dataMutex);
141 
142  robots.clear();
143 
144  if (useWorkingMemory)
145  {
146  workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(workingMemoryName);
147  objectInstancesPrx = workingMemoryPrx->getObjectInstancesSegment();
148  agentInstancesPrx = workingMemoryPrx->getAgentInstancesSegment();
149  fileManager.reset(new memoryx::GridFileManager(workingMemoryPrx->getCommonStorage()));
150  for (memoryx::AgentInstanceBasePtr& agent : agentInstancesPrx->getAllAgentInstances())
151  {
152  RobotStateComponentInterfacePrx robotStateComponentPrx = agent->getSharedRobot()->getRobotStateComponent();
154 
155  robots.push_back({robot, robotStateComponentPrx});
156  }
157  }
158  else
159  {
160  RobotStateComponentInterfacePrx robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
161 
163 
164  RobotPair r = {robot, robotStateComponentPrx};
165  robots.push_back(r);
166  }
167 
168  {
169  std::unique_lock lock(connectedMutex);
170  connected = true;
171  }
172 
173 
174  for (SceneObjectPair& pair : sceneObjectPairs)
175  {
176  if (!resolveCollisionPair(pair))
177  {
178  std::ostringstream nodeNames1Str;
179  std::copy(pair.nodeNames1.begin(), pair.nodeNames1.end() - 1, std::ostream_iterator<std::string>(nodeNames1Str, ","));
180  nodeNames1Str << pair.nodeNames1.back();
181 
182  std::ostringstream nodeNames2Str;
183  std::copy(pair.nodeNames2.begin(), pair.nodeNames2.end() - 1, std::ostream_iterator<std::string>(nodeNames2Str, ","));
184  nodeNames2Str << pair.nodeNames2.back();
185  ARMARX_WARNING << "Could not resolve collision pair: {" << pair.robotName1 << (pair.robotName1 == "" ? "" : ":") << "(" << nodeNames1Str.str() << ")," << pair.robotName2 << (pair.robotName2 == "" ? "" : ":") << "(" << nodeNames2Str.str() << ")," << pair.warningDistance << "}" << std::endl;
186  }
187  }
188  }
189 
190  collisionListenerPrx = getTopic<CollisionListenerPrx>(getProperty<std::string>("CollisionListenerTopicName").getValue());
191  distanceListenerPrx = getTopic<DistanceListenerPrx>(getProperty<std::string>("DistanceListenerTopicName").getValue());
192 
193  if (!reportTask)
194  {
195  reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
196  }
197 
198  reportTask->start();
199 
200  if (useDebugDrawer)
201  {
202  debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
203 
204  if (!debugDrawerTopicPrx)
205  {
206  ARMARX_ERROR << "Failed to obtain debug drawer proxy." << std::endl;
207  useDebugDrawer = false;
208  }
209  }
210 }
211 
212 
214 {
215  {
216  std::unique_lock lock(connectedMutex);
217  connected = false;
218  }
219  reportTask->stop();
220 
221  std::scoped_lock lockData(dataMutex);
222  for (SceneObjectPair& pair : sceneObjectPairs)
223  {
224  pair.objects1 = VirtualRobot::SceneObjectSetPtr();
225  pair.objects2 = VirtualRobot::SceneObjectSetPtr();
226  }
227  robots.clear();
228 
229  if (useDebugDrawer && debugDrawerTopicPrx)
230  {
231  debugDrawerTopicPrx->clearLayer("distanceVisu");
232  debugDrawerTopicPrx->removeLayer("distanceVisu");
233  }
234 }
235 
236 
238 {
239 
240 }
241 
243 {
246 }
247 
248 bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pair)
249 {
250  std::shared_lock lockConnected(connectedMutex);
251  if (!connected)
252  {
253  return false;
254  }
255 
256  if (pair.usesNodeSet1 && pair.nodeSetName1 != "")
257  {
258  pair.nodeNames1.clear();
259  if (pair.robotName1 != "")
260  {
261  for (RobotPair& robotPair : robots)
262  {
263  if (robotPair.robot->getName() == pair.robotName1)
264  {
265  VirtualRobot::SceneObjectSetPtr set = robotPair.robot->getRobotNodeSet(pair.nodeSetName1);
266  if (set)
267  {
268  for (VirtualRobot::SceneObjectPtr so : set->getSceneObjects())
269  {
270  pair.nodeNames1.push_back(so->getName());
271  }
272  }
273  else
274  {
275  ARMARX_WARNING << "Robot \"" << pair.robotName1 << "\" has no set with name \"" << pair.nodeSetName1 << "\"" << std::endl;
276  }
277  break;
278  }
279  }
280  }
281  else
282  {
283  ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName1 << "\"" << std::endl;
284  }
285  }
286 
287  VirtualRobot::SceneObjectSetPtr objects1(new VirtualRobot::SceneObjectSet());
288  for (std::string& nodeName : pair.nodeNames1)
289  {
290  VirtualRobot::SceneObjectPtr sc = getSceneObject(pair.robotName1, nodeName);
291  if (!sc)
292  {
293  ARMARX_DEBUG << "failed to get object: \"" << pair.robotName1 << "\" : \"" << nodeName << "\"" << std::endl;
294  return false;
295  }
296  if (!sc->getCollisionModel())
297  {
298  ARMARX_WARNING << pair.robotName1 << (pair.robotName1 == "" ? "" : ".") << nodeName << " does not have a collision model. Ignoring..." << std::endl;
299  continue;
300  }
301  objects1->addSceneObject(sc);
302  }
303 
304  if (pair.usesNodeSet2 && pair.nodeSetName2 != "")
305  {
306  pair.nodeNames2.clear();
307  if (pair.robotName2 != "")
308  {
309  for (RobotPair& robotPair : robots)
310  {
311  if (robotPair.robot->getName() == pair.robotName2)
312  {
313  VirtualRobot::SceneObjectSetPtr set = robotPair.robot->getRobotNodeSet(pair.nodeSetName2);
314  if (set)
315  {
316  for (VirtualRobot::SceneObjectPtr so : set->getSceneObjects())
317  {
318  pair.nodeNames2.push_back(so->getName());
319  }
320  }
321  else
322  {
323  ARMARX_WARNING << "Robot \"" << pair.robotName2 << "\" has no set with name \"" << pair.nodeSetName2 << "\"" << std::endl;
324  }
325  break;
326  }
327  }
328  }
329  else
330  {
331  ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName2 << "\"" << std::endl;
332  }
333  }
334 
335  VirtualRobot::SceneObjectSetPtr objects2(new VirtualRobot::SceneObjectSet());
336  for (std::string& nodeName : pair.nodeNames2)
337  {
338  VirtualRobot::SceneObjectPtr sc = getSceneObject(pair.robotName2, nodeName);
339  if (!sc)
340  {
341  ARMARX_DEBUG << "failed to get object: \"" << pair.robotName2 << "\" : \"" << nodeName << "\"" << std::endl;
342  return false;
343  }
344  if (!sc->getCollisionModel())
345  {
346  ARMARX_WARNING << pair.robotName2 << (pair.robotName2 == "" ? "" : ".") << nodeName << " does not have a collision model. Ignoring..." << std::endl;
347  continue;
348  }
349  objects2->addSceneObject(sc);
350  }
351 
352  if (!objects1 || !objects2 || objects1->getSize() == 0 || objects2->getSize() == 0)
353  {
354  return false;
355  }
356 
357  pair.objects1 = objects1;
358  pair.objects2 = objects2;
359  return true;
360 }
361 
362 VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObject(const std::string& robotName, const std::string& nodeName)
363 {
364  if (robotName.size() == 0 && useWorkingMemory)
365  {
366  ARMARX_DEBUG << "Get object from WorkingMemory: " << nodeName << std::endl;
367  return getSceneObjectFromWorkingMemory(nodeName);
368  }
369  else if (robotName.size() == 0) //&& !useWorkingMemory
370  {
371  //get RobotNode from first robot (should be only one)
372  ARMARX_DEBUG << "Get object from first robot: " << nodeName << std::endl;
373  return robots[0].robot->getRobotNode(nodeName);
374  }
375  else
376  {
377  ARMARX_DEBUG << "Searching for matching robot: " << robotName << std::endl;
378  for (RobotPair& r : robots)
379  {
380  if (r.robot->getName() == robotName)
381  {
382  ARMARX_DEBUG << "Get object from robot: " << nodeName << std::endl;
383  return r.robot->getRobotNode(nodeName);
384  }
385  else
386  {
387  ARMARX_DEBUG << r.robot->getName() << " != " << robotName << std::endl;
388  }
389  }
390  }
391 
393 }
394 
395 bool armarx::CollisionCheckerComponent::parseNodeSet(const std::string& setAsString, std::string& robotName, std::vector<std::string>& nodeNames, bool& usesNodeSet, std::string& nodeSetName)
396 {
397  std::vector<std::string> split = simox::alg::split(setAsString, ":");
398 
399  std::string nodeNamesString;
400  switch (split.size())
401  {
402  case 1:
403  robotName = "";
404  nodeNamesString = split[0];
405  break;
406  case 2:
407  robotName = split[0];
408  nodeNamesString = split[1];
409  break;
410  default:
411  return false;
412  }
413 
414  if (nodeNamesString[0] != '{' && nodeNamesString[0] != '(' && nodeNamesString[0] != '[')
415  {
416  usesNodeSet = true;
417  nodeSetName = nodeNamesString;
418  nodeNames.clear();
419  }
420  else
421  {
422  usesNodeSet = false;
423  nodeSetName = "";
424  while ((nodeNamesString[0] == '{' && nodeNamesString[nodeNamesString.size() - 1] == '}') ||
425  (nodeNamesString[0] == '(' && nodeNamesString[nodeNamesString.size() - 1] == ')') ||
426  (nodeNamesString[0] == '[' && nodeNamesString[nodeNamesString.size() - 1] == ']'))
427  {
428  nodeNamesString = nodeNamesString.substr(1, nodeNamesString.size() - 2);
429  }
430 
431  ARMARX_CHECK_EXPRESSION(nodeNamesString.size() > 0);
432 
433  nodeNames = simox::alg::split(nodeNamesString, ",");
434  }
435  return true;
436 }
437 
438 VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFromWorkingMemory(const std::string& name)
439 {
440  for (VirtualRobot::SceneObjectPtr& sc : workingMemorySceneObjects)
441  {
442  if (sc->getName() == name)
443  {
444  return sc;
445  }
446  }
447  const memoryx::EntityBasePtr entityBase = objectInstancesPrx->getEntityByName(name);
448  const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
449 
450  if (!object)
451  {
452  ARMARX_ERROR << "Could not get object with name " << name << std::endl;
454  }
455 
456  const std::string className = object->getMostProbableClass();
457 
458  memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(className);
459 
460  if (!classes.size())
461  {
462  ARMARX_INFO_S << "No classes for most probable class '" << className << "' of object '" << object->getName() << "' with name " << name;
464  }
465 
466  memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(classes.at(0));
467  const PosePtr objectPose = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr {new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
468 
469  if (!objectClass)
470  {
471  ARMARX_ERROR << "Can't use object class with ice id " << classes.at(0)->ice_id() << std::endl;
473  }
474 
476  VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
478  std::string moName = orgMo->getName();
479  VirtualRobot::ManipulationObjectPtr mo = orgMo->clone(moName);
480 
481  //move the object to the given position
482  if (!objectPose)
483  {
484  ARMARX_ERROR << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose." << std::endl;
486  }
487  mo->setGlobalPose(objectPose->toEigen());
488  mo->setName(name); //make shure the name of the scene object is equal to the requested name
489 
490  workingMemorySceneObjects.push_back(mo);
491  return mo;
492 }
493 
494 void armarx::CollisionCheckerComponent::synchronizeObjectsFromWorkingMemory()
495 {
496  std::scoped_lock lockPosition(wmPositionMutex);
497  for (VirtualRobot::SceneObjectPtr& sc : workingMemorySceneObjects)
498  {
499  auto it = currentPositions.find(sc->getName());
500  if (it != currentPositions.end())
501  {
502  sc->setGlobalPose(it->second->toEigen());
503  }
504  }
505 }
506 
507 void armarx::CollisionCheckerComponent::reportDistancesAndCollisions()
508 {
509  std::scoped_lock lockData(dataMutex);
510  for (RobotPair& r : robots)
511  {
512  armarx::RemoteRobot::synchronizeLocalClone(r.robot, r.robotStateComponentPrx->getSynchronizedRobot());
513  }
514  synchronizeObjectsFromWorkingMemory();
515 
516  for (SceneObjectPair& pair : sceneObjectPairs)
517  {
518  if (!pair.objects1 || !pair.objects2)
519  {
520  continue;
521  }
522  double distance;
523  if (useDebugDrawer && debugDrawerTopicPrx)
524  {
525  std::ostringstream nodeNames1Str;
526  std::copy(pair.nodeNames1.begin(), pair.nodeNames1.end() - 1, std::ostream_iterator<std::string>(nodeNames1Str, ","));
527  nodeNames1Str << pair.nodeNames1.back();
528 
529  std::ostringstream nodeNames2Str;
530  std::copy(pair.nodeNames2.begin(), pair.nodeNames2.end() - 1, std::ostream_iterator<std::string>(nodeNames2Str, ","));
531  nodeNames2Str << pair.nodeNames2.back();
532 
533  Eigen::Vector3f p1, p2;
534  distance = collisionChecker->calculateDistance(pair.objects1, pair.objects2, p1, p2);
535  Vector3BasePtr p1a(new Vector3(p1)), p2a(new Vector3(p2));
536  DrawColor color;
537  if (distance <= pair.warningDistance)
538  {
539  color.r = 1;
540  color.g = 1;
541  color.b = 0;
542  }
543  else
544  {
545  color.r = 0;
546  color.g = 0.9;
547  color.b = 0;
548  }
549  debugDrawerTopicPrx->setLineVisu("distanceVisu", pair.robotName1 + (pair.robotName1 == "" ? "" : ": ") + nodeNames1Str.str() + " - " + pair.robotName2 + (pair.robotName2 == "" ? "" : ": ") + nodeNames2Str.str(), p1a, p2a, 1, color);
550  }
551  else
552  {
553  distance = collisionChecker->calculateDistance(pair.objects1, pair.objects2);
554  }
555 
556  distanceListenerPrx->reportDistance(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
557 
558  if (distance <= pair.warningDistance)
559  {
560  collisionListenerPrx->reportCollisionWarning(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
561  }
562 
563  if (collisionChecker->checkCollision(pair.objects1, pair.objects2))
564  {
565  collisionListenerPrx->reportCollision(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
566  }
567  }
568 }
569 
570 void CollisionCheckerComponent::addCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, double warningDistance, const Ice::Current&)
571 {
572  addCollisionPair(robotName1, nodeNames1, false, "", robotName2, nodeNames2, false, "", warningDistance);
573 }
574 
575 void CollisionCheckerComponent::addCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const bool usesNodeSet1, const std::string& nodeSetName1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const bool usesNodeSet2, const std::string& nodeSetName2, double warningDistance)
576 {
577  if (hasCollisionPair(robotName1, nodeNames1, robotName2, nodeNames2))
578  {
579  return;
580  }
581  SceneObjectPair pair = {VirtualRobot::SceneObjectSetPtr(), robotName1, nodeNames1, usesNodeSet1, nodeSetName1, VirtualRobot::SceneObjectSetPtr(), robotName2, nodeNames2, usesNodeSet2, nodeSetName2, warningDistance};
582  std::scoped_lock lockData(dataMutex);
583  resolveCollisionPair(pair);
584  sceneObjectPairs.push_back(pair);
585 }
586 
587 void CollisionCheckerComponent::removeCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&)
588 {
589  std::scoped_lock lockData(dataMutex);
590  for (auto it = sceneObjectPairs.begin(); it != sceneObjectPairs.end(); it++)
591  {
592  if ((it->robotName1 == robotName1 && it->nodeNames1 == nodeNames1 &&
593  it->robotName2 == robotName2 && it->nodeNames2 == nodeNames2) ||
594  (it->robotName1 == robotName2 && it->nodeNames1 == nodeNames2 &&
595  it->robotName2 == robotName1 && it->nodeNames2 == nodeNames1))
596  {
597  sceneObjectPairs.erase(it);
598  break;
599  }
600  }
601 }
602 
603 bool CollisionCheckerComponent::hasCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&) const
604 {
605  std::scoped_lock lockData(dataMutex);
606  for (const SceneObjectPair& pair : sceneObjectPairs)
607  {
608  if ((pair.robotName1 == robotName1 && pair.nodeNames1 == nodeNames1 &&
609  pair.robotName2 == robotName2 && pair.nodeNames2 == nodeNames2) ||
610  (pair.robotName1 == robotName2 && pair.nodeNames1 == nodeNames2 &&
611  pair.robotName2 == robotName1 && pair.nodeNames2 == nodeNames1))
612  {
613  return true;
614  }
615  }
616  return false;
617 }
618 
619 void CollisionCheckerComponent::setWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, double warningDistance, const Ice::Current&)
620 {
621  std::scoped_lock lockData(dataMutex);
622  for (SceneObjectPair& pair : sceneObjectPairs)
623  {
624  if ((pair.robotName1 == robotName1 && pair.nodeNames1 == nodeNames1 &&
625  pair.robotName2 == robotName2 && pair.nodeNames2 == nodeNames2) ||
626  (pair.robotName1 == robotName2 && pair.nodeNames1 == nodeNames2 &&
627  pair.robotName2 == robotName1 && pair.nodeNames2 == nodeNames1))
628  {
629  pair.warningDistance = warningDistance;
630  break;
631  }
632  }
633 }
634 
635 double CollisionCheckerComponent::getWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&) const
636 {
637  std::scoped_lock lockData(dataMutex);
638  for (const SceneObjectPair& pair : sceneObjectPairs)
639  {
640  if ((pair.robotName1 == robotName1 && pair.nodeNames1 == nodeNames1 &&
641  pair.robotName2 == robotName2 && pair.nodeNames2 == nodeNames2) ||
642  (pair.robotName1 == robotName2 && pair.nodeNames1 == nodeNames2 &&
643  pair.robotName2 == robotName1 && pair.nodeNames2 == nodeNames1))
644  {
645  return pair.warningDistance;
646  }
647  }
648  return INFINITY;
649 }
650 
651 CollisionPairList CollisionCheckerComponent::getAllCollisionPairs(const Ice::Current&) const
652 {
653  std::scoped_lock lockData(dataMutex);
654  CollisionPairList list;
655  for (const SceneObjectPair& pair : sceneObjectPairs)
656  {
657  armarx::CollisionPair p = {pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, pair.warningDistance};
658  list.push_back(p);
659  }
660  return list;
661 }
662 
663 int CollisionCheckerComponent::getInterval(const Ice::Current&) const
664 {
665  return interval;
666 }
667 
668 void CollisionCheckerComponent::setInterval(int interval, const Ice::Current&)
669 {
670  std::shared_lock lockConnected(connectedMutex);
671  this->interval = interval;
672  if (connected && reportTask && reportTask->isRunning())
673  {
674  reportTask->stop();
675  reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
676  reportTask->start();
677  }
678  else
679  {
680  reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
681  }
682 }
683 
684 void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentName, const memoryx::EntityBasePtr& entity, const Ice::Current&)
685 {
686  if (segmentName == objectInstancesPrx->getSegmentName())
687  {
688  std::scoped_lock lockData(dataMutex);
689  for (SceneObjectPair& pair : sceneObjectPairs)
690  {
691  if ((!pair.objects1 || !pair.objects2) &&
692  ((pair.robotName1 == "" && std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) != pair.nodeNames1.end()) ||
693  (pair.robotName2 == "" && std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) != pair.nodeNames2.end())))
694  {
695  resolveCollisionPair(pair);
696  }
697  }
698  }
699  else if (segmentName == agentInstancesPrx->getSegmentName())
700  {
701  std::scoped_lock lockData(dataMutex);
702  memoryx::AgentInstanceBasePtr agent = agentInstancesPrx->getAgentInstanceById(entity->getId());
703  RobotStateComponentInterfacePrx robotStateComponentPrx = agent->getSharedRobot()->getRobotStateComponent();
705 
706  robots.push_back({robot, robotStateComponentPrx});
707 
708 
709  for (SceneObjectPair& pair : sceneObjectPairs)
710  {
711  if ((!pair.objects1 || !pair.objects2) &&
712  (pair.robotName1 == entity->getName() ||
713  pair.robotName2 == entity->getName()))
714  {
715  resolveCollisionPair(pair);
716  }
717  }
718  }
719 }
720 
721 void CollisionCheckerComponent::reportEntityUpdated(const std::string& segmentName, const memoryx::EntityBasePtr& entityOld, const memoryx::EntityBasePtr& entityNew, const Ice::Current&)
722 {
723  if (segmentName == objectInstancesPrx->getSegmentName())
724  {
725  std::scoped_lock lockPosition(wmPositionMutex);
726  const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityNew);
727  currentPositions[entityNew->getName()] = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr {new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
728  }
729 }
730 
731 void CollisionCheckerComponent::reportEntityRemoved(const std::string& segmentName, const memoryx::EntityBasePtr& entity, const Ice::Current&)
732 {
733  if (segmentName == objectInstancesPrx->getSegmentName())
734  {
735  std::scoped_lock lockData(dataMutex);
736  for (SceneObjectPair& pair : sceneObjectPairs)
737  {
738  if ((pair.robotName1 == "" && std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) != pair.nodeNames1.end()) ||
739  (pair.robotName2 == "" && std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) != pair.nodeNames2.end()))
740  {
741  pair.objects1.reset();
742  pair.objects2.reset();
743  }
744  }
745  for (auto it = workingMemorySceneObjects.begin(); it != workingMemorySceneObjects.end(); ++it)
746  {
747  if ((*it)->getName() == entity->getName())
748  {
749  workingMemorySceneObjects.erase(it);
750  break;
751  }
752  }
753  }
754  else if (segmentName == agentInstancesPrx->getSegmentName())
755  {
756  std::scoped_lock lockData(dataMutex);
757  for (auto it = robots.begin(); it != robots.end(); ++it)
758  {
759  if ((*it).robot->getName() == entity->getName())
760  {
761  robots.erase(it);
762  break;
763  }
764  }
765 
766  for (SceneObjectPair& pair : sceneObjectPairs)
767  {
768  if (pair.robotName1 == entity->getName() ||
769  pair.robotName2 == entity->getName())
770  {
771  pair.objects1.reset();
772  pair.objects2.reset();
773  }
774  }
775  }
776 }
777 
778 void CollisionCheckerComponent::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
779 {
780  if (segmentName == objectInstancesPrx->getSegmentName())
781  {
782  std::scoped_lock lockData(dataMutex);
783  for (SceneObjectPair& pair : sceneObjectPairs)
784  {
785  if (!pair.objects1 || !pair.objects2)
786  {
787  resolveCollisionPair(pair);
788  }
789  }
790  }
791 }
792 
793 void CollisionCheckerComponent::reportSnapshotCompletelyLoaded(const Ice::Current& c)
794 {
795 
796 }
797 
798 void CollisionCheckerComponent::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
799 {
800  if (segmentName == objectInstancesPrx->getSegmentName())
801  {
802  std::scoped_lock lockData(dataMutex);
803  for (SceneObjectPair& pair : sceneObjectPairs)
804  {
805  if (pair.robotName1 == "" || pair.robotName2 == "")
806  {
807  pair.objects1.reset();
808  pair.objects2.reset();
809  }
810  }
811  }
812 }
813 
armarx::CollisionCheckerComponent::setWarningDistance
void setWarningDistance(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, double warningDistance, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:619
RemoteRobot.h
armarx::CollisionCheckerComponent::onInitComponent
void onInitComponent() override
Definition: CollisionCheckerComponent.cpp:56
armarx::CollisionCheckerComponent::getWarningDistance
double getWarningDistance(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:635
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:448
robots
std::vector< VirtualRobot::RobotPtr > robots
Definition: VisualizationRobot.cpp:85
armarx::RemoteRobot::createLocalCloneFromFile
static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
This is a convenience function for createLocalClone, which automatically gets the filename from the R...
Definition: RemoteRobot.cpp:441
list
list(APPEND SOURCES ${QT_RESOURCES}) set(COMPONENT_LIBS ArmarXGui ArmarXCoreObservers ArmarXCoreEigen3Variants PlotterController $
Definition: CMakeLists.txt:49
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::CollisionCheckerComponent::hasCollisionPair
bool hasCollisionPair(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:603
so
linux gnu libIceDB so
Definition: CMakeLists.txt:7
ObjectClass.h
IceInternal::Handle< ObjectClass >
armarx::CollisionCheckerComponent::onExitComponent
void onExitComponent() override
Definition: CollisionCheckerComponent.cpp:237
armarx::CollisionCheckerComponent::CollisionCheckerComponent
CollisionCheckerComponent()
Definition: CollisionCheckerComponent.cpp:50
armarx::CollisionCheckerComponent::getInterval
int getInterval(const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:663
copy
Use of this software is granted under one of the following two to be chosen freely by the user Boost Software License Version Marcin Kalicinski Permission is hereby free of to any person or organization obtaining a copy of the software and accompanying documentation covered by this and transmit the and to prepare derivative works of the and to permit third parties to whom the Software is furnished to do all subject to the including the above license this restriction and the following must be included in all copies of the in whole or in and all derivative works of the unless such copies or derivative works are solely in the form of machine executable object code generated by a source language processor THE SOFTWARE IS PROVIDED AS WITHOUT WARRANTY OF ANY EXPRESS OR INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF FITNESS FOR A PARTICULAR TITLE AND NON INFRINGEMENT IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER WHETHER IN TORT OR ARISING OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE The MIT Marcin Kalicinski Permission is hereby free of to any person obtaining a copy of this software and associated documentation to deal in the Software without including without limitation the rights to copy
Definition: license.txt:39
armarx::interval
Interval< T > interval(T lo, T hi)
Definition: OccupancyGrid.h:26
armarx::CollisionCheckerComponent::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: CollisionCheckerComponent.cpp:242
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
CollisionCheckerComponent.h
MemoryXCoreObjectFactories.h
armarx::CollisionCheckerComponent::addCollisionPair
void addCollisionPair(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, double warningDistance, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:570
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::CollisionCheckerComponent::setInterval
void setInterval(int interval, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:668
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
armarx::CollisionCheckerComponentPropertyDefinitions
Definition: CollisionCheckerComponent.h:49
armarx::CollisionCheckerComponent::getAllCollisionPairs
CollisionPairList getAllCollisionPairs(const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:651
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
CMakePackageFinder.h
SimoxObjectWrapper.h
armarx::Pose
The Pose class.
Definition: Pose.h:242
armarx::CollisionCheckerComponent::onDisconnectComponent
void onDisconnectComponent() override
Definition: CollisionCheckerComponent.cpp:213
ObjectInstance.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
set
set(LIBS ArmarXCoreInterfaces ${CMAKE_THREAD_LIBS_INIT} ${dl_LIBRARIES} ${rt_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} BoostAssertionHandler ArmarXCPPUtility SimoxUtility) set(LIB_FILES ArmarXManager.cpp ArmarXMultipleObjectsScheduler.cpp ArmarXObjectScheduler.cpp ManagedIceObject.cpp ManagedIceObjectPlugin.cpp Component.cpp ComponentPlugin.cpp IceGridAdmin.cpp ArmarXObjectObserver.cpp IceManager.cpp PackagePath.cpp RemoteReferenceCount.cpp logging/LoggingUtil.cpp logging/Logging.cpp logging/LogSender.cpp logging/ArmarXLogBuf.cpp system/ArmarXDataPath.cpp system/DynamicLibrary.cpp system/ProcessWatcher.cpp system/FactoryCollectionBase.cpp system/cmake/CMakePackageFinder.cpp system/cmake/CMakePackageFinderCache.cpp system/cmake/ArmarXPackageToolInterface.cpp system/RemoteObjectNode.cpp services/sharedmemory/HardwareId.cpp services/tasks/RunningTask.cpp services/tasks/ThreadList.cpp services/tasks/ThreadPool.cpp services/profiler/Profiler.cpp services/profiler/FileLoggingStrategy.cpp services/profiler/IceLoggingStrategy.cpp application/Application.cpp application/ApplicationOptions.cpp application/ApplicationProcessFacet.cpp application/ApplicationNetworkStats.cpp application/properties/PropertyUser.cpp application/properties/Property.cpp application/properties/PropertyDefinition.cpp application/properties/PropertyDefinitionContainer.cpp application/properties/PropertyDefinitionHelpFormatter.cpp application/properties/PropertyDefinitionConfigFormatter.cpp application/properties/PropertyDefinitionBriefHelpFormatter.cpp application/properties/PropertyDefinitionXmlFormatter.cpp application/properties/PropertyDefinitionDoxygenFormatter.cpp application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.cpp application/properties/PropertyDefinitionContainerBriefHelpFormatter.cpp application/properties/IceProperties.cpp exceptions/Exception.cpp exceptions/local/UnexpectedEnumValueException.cpp util/FileSystemPathBuilder.cpp util/StringHelpers.cpp util/IceReportSkipper.cpp util/Throttler.cpp util/distributed/AMDCallbackCollection.cpp util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.cpp util/distributed/RemoteHandle/RemoteHandle.cpp util/distributed/RemoteHandle/RemoteHandleControlBlock.cpp time/ice_conversions.cpp time/json_conversions.cpp time/CallbackWaitLock.cpp time/Clock.cpp time/ClockType.cpp time/ClockTypeNames.cpp time/CycleUtil.cpp time/DateTime.cpp time/Duration.cpp time/Frequency.cpp time/LocalTimeServer.cpp time/Metronome.cpp time/ScopedStopWatch.cpp time/StopWatch.cpp time/Timer.cpp time/TimeKeeper.cpp time/TimeUtil.cpp csv/CsvWriter.cpp csv/CsvReader.cpp eigen/conversions.cpp eigen/ice_conversions.cpp) set(LIB_HEADERS ArmarXManager.h ArmarXDummyManager.h ArmarXMultipleObjectsScheduler.h ArmarXObjectObserver.h ArmarXObjectScheduler.h ArmarXFwd.h Component.h ComponentPlugin.h ComponentFactories.h CoreObjectFactories.h IceGridAdmin.h IceManager.h IceManagerImpl.h json_conversions.h ManagedIceObject.h ManagedIceObjectPlugin.h ManagedIceObjectImpl.h ManagedIceObjectDependency.h ManagedIceObjectRegistryInterface.h PackagePath.h RemoteReferenceCount.h system/ImportExport.h system/ImportExportComponent.h system/AbstractFactoryMethod.h system/FactoryCollectionBase.h system/Synchronization.h system/ArmarXDataPath.h system/DynamicLibrary.h system/ProcessWatcher.h system/ConditionSynchronization.h system/cmake/CMakePackageFinder.h system/cmake/CMakePackageFinderCache.h system/cmake/FindPackageX.cmake system/cmake/ArmarXPackageToolInterface.h system/RemoteObjectNode.h logging/LoggingUtil.h logging/LogSender.h logging/Logging.h logging/ArmarXLogBuf.h logging/SpamFilterData.h services/tasks/RunningTask.h services/tasks/PeriodicTask.h services/tasks/ThreadList.h services/tasks/TaskUtil.h services/tasks/ThreadPool.h services/sharedmemory/SharedMemoryProvider.h services/sharedmemory/SharedMemoryConsumer.h services/sharedmemory/IceSharedMemoryProvider.h services/sharedmemory/IceSharedMemoryConsumer.h services/sharedmemory/HardwareIdentifierProvider.h services/sharedmemory/HardwareId.h services/sharedmemory/exceptions/SharedMemoryExceptions.h services/profiler/Profiler.h services/profiler/LoggingStrategy.h services/profiler/FileLoggingStrategy.h services/profiler/IceLoggingStrategy.h application/Application.h application/ApplicationOptions.h application/ApplicationProcessFacet.h application/ApplicationNetworkStats.h application/properties/forward_declarations.h application/properties/Properties.h application/properties/Property.h application/properties/PluginEigen.h application/properties/PluginEnumNames.h application/properties/PluginCfgStruct.h application/properties/PluginAll.h application/properties/PropertyUser.h application/properties/PropertyDefinition.h application/properties/PropertyDefinition.hpp application/properties/PropertyDefinitionInterface.h application/properties/PropertyDefinitionContainer.h application/properties/PropertyDefinitionFormatter.h application/properties/PropertyDefinitionContainerFormatter.h application/properties/PropertyDefinitionConfigFormatter.h application/properties/PropertyDefinitionHelpFormatter.h application/properties/PropertyDefinitionBriefHelpFormatter.h application/properties/PropertyDefinitionXmlFormatter.h application/properties/PropertyDefinitionDoxygenFormatter.h application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.h application/properties/PropertyDefinitionContainerBriefHelpFormatter.h application/properties/ProxyPropertyDefinition.h application/properties/IceProperties.h exceptions/Exception.h exceptions/LocalException.h exceptions/local/DynamicLibraryException.h exceptions/local/ExpressionException.h exceptions/local/FileIOException.h exceptions/local/InvalidPropertyValueException.h exceptions/local/MissingRequiredPropertyException.h exceptions/local/PropertyInheritanceCycleException.h exceptions/local/ProxyNotInitializedException.h exceptions/local/UnexpectedEnumValueException.h exceptions/local/UnmappedValueException.h exceptions/local/ValueRangeExceededException.h exceptions/user/NotImplementedYetException.h rapidxml/rapidxml.hpp rapidxml/rapidxml_print.hpp rapidxml/rapidxml_iterators.hpp rapidxml/rapidxml_utils.hpp rapidxml/wrapper/RapidXmlReader.h rapidxml/wrapper/RapidXmlWriter.h rapidxml/wrapper/DefaultRapidXmlReader.h rapidxml/wrapper/MultiNodeRapidXMLReader.h util/IceBlobToObject.h util/ObjectToIceBlob.h util/FileSystemPathBuilder.h util/FiniteStateMachine.h util/StringHelpers.h util/StringHelperTemplates.h util/algorithm.h util/OnScopeExit.h util/Predicates.h util/Preprocessor.h util/PropagateConst.h util/Registrar.h util/TemplateMetaProgramming.h util/TripleBuffer.h util/IceReportSkipper.h util/Throttler.h util/distributed/AMDCallbackCollection.h util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.h util/distributed/RemoteHandle/RemoteHandle.h util/distributed/RemoteHandle/RemoteHandleControlBlock.h util/SimpleStatemachine.h time.h time_minimal.h time/forward_declarations.h time/ice_conversions.h time/json_conversions.h time/CallbackWaitLock.h time/Clock.h time/ClockType.h time/ClockTypeNames.h time/CycleUtil.h time/DateTime.h time/Duration.h time/Frequency.h time/LocalTimeServer.h time/Metronome.h time/ScopedStopWatch.h time/StopWatch.h time/Timer.h time/TimeUtil.h time/TimeKeeper.h csv/CsvWriter.h csv/CsvReader.h eigen/conversions.h eigen/ice_conversions.h ice_conversions.h ice_conversions/ice_conversions_boost_templates.h ice_conversions/ice_conversions_templates.h ice_conversions/ice_conversions_templates.tpp $
Definition: CMakeLists.txt:12
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::CollisionCheckerComponent::removeCollisionPair
void removeCollisionPair(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:587
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:42
MemoryXTypesObjectFactories.h
AgentInstance.h
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::CollisionCheckerComponent::onConnectComponent
void onConnectComponent() override
Definition: CollisionCheckerComponent.cpp:137
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36