VoxelGridMappingProvider.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::VoxelGridMappingProvider
17  * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "VoxelGridMappingProvider.h"
24 
25 #include <pcl/common/transforms.h>
26 #include <pcl/filters/approximate_voxel_grid.h>
27 #include <pcl/filters/frustum_culling.h>
28 #include <pcl/filters/passthrough.h>
29 #include <pcl/filters/uniform_sampling.h>
30 #include <pcl/filters/voxel_grid.h>
31 #include <pcl/io/pcd_io.h>
32 #include <pcl/point_types.h>
33 
34 #include <ArmarXCore/core/util/IceReportSkipper.h>
35 #include <ArmarXCore/core/util/StringHelperTemplates.h>
36 #include <ArmarXCore/core/util/algorithm.h>
37 
38 #include <RobotAPI/components/DebugDrawer/DebugDrawerUtils.h>
39 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
40 
41 //#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
42 #include <VirtualRobot/CollisionDetection/CDManager.h>
43 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
44 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
45 #include <VirtualRobot/Visualization/TriMeshUtils.h>
46 
47 #include <VisionX/components/pointcloud_core/PointCloudConversions.h>
48 #include <VisionX/interface/components/Calibration.h>
49 
50 #include <MotionPlanning/CSpace/CSpaceSampled.h>
51 #include <MotionPlanning/Planner/BiRrt.h>
52 #include <MotionPlanning/PostProcessing/ShortcutProcessor.h>
53 
54 
55 using namespace armarx;
56 
57 namespace visionx
58 {
59  VoxelGridMappingProvider::VoxelGridMappingProvider()
60  {
61  }
62 
63  std::string
64  VoxelGridMappingProvider::getDefaultName() const
65  {
66  return "VoxelGridMappingProvider";
67  }
68 
69  PropertyDefinitionsPtr
70  VoxelGridMappingProvider::createPropertyDefinitions()
71  {
72  return PropertyDefinitionsPtr(
73  new VoxelGridMappingProviderPropertyDefinitions(getConfigIdentifier()));
74  }
75 
76  void
77  VoxelGridMappingProvider::onInitPointCloudProcessor()
78  {
79  skipper.reset(
80  new IceReportSkipper(getProperty<float>("PointCloudStorageFrequency").getValue()));
81  cycleKeeper.reset(new CycleUtil(1000.0f / getProperty<float>("UpdateRate").getValue()));
82  providerName = getProperty<std::string>("providerName").getValue();
83  sourceFrameName = getProperty<std::string>("sourceFrameName").getValue();
84  updateBoundaries();
85  gridLeafSize = getProperty<float>("GridSize");
86 
87  usingPointCloudProvider(providerName);
88  accumulatedPointCloud.reset(new pcl::PointCloud<PointType>());
89  if (!getProperty<std::string>("PointCloudLoadFilepath").getValue().empty())
90  {
91  ARMARX_INFO << "Loading pointcloud from "
92  << getProperty<std::string>("PointCloudLoadFilepath").getValue();
93  pcl::io::loadPCDFile(getProperty<std::string>("PointCloudLoadFilepath"),
94  *accumulatedPointCloud);
95  }
96  }
97 
98  void
99  VoxelGridMappingProvider::onConnectPointCloudProcessor()
100  {
101  if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
102  {
103  robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
104  getProperty<std::string>("RobotStateComponentName").getValue());
105  auto localRobot = RemoteRobot::createLocalCloneFromFile(
106  robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
107  robotPool.reset(new RobotPool(localRobot, 3));
108  }
109  dd = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
110  collectingPointClouds = getProperty<bool>("ActivateOnStartUp");
111  octtree.reset(new pcl::octree::OctreePointCloud<PointType>(gridLeafSize));
112 
113  if (sourceFrameName.empty())
114  {
115 
116  visionx::ReferenceFrameInterfacePrx proxy =
117  getProxy<visionx::ReferenceFrameInterfacePrx>(providerName);
118  if (proxy)
119  {
120  sourceFrameName = proxy->getReferenceFrame();
121  ARMARX_INFO << "source frame name is " << sourceFrameName;
122  }
123  }
124 
125  enableResultPointClouds<PointType>();
126  }
127 
128  void
129  VoxelGridMappingProvider::onExitPointCloudProcessor()
130  {
131  }
132 
133  void
134  VoxelGridMappingProvider::updateBoundaries()
135  {
136  auto limits = Split(getProperty<std::string>("BoundingBox").getValue(), ";", true);
137  int i = 0;
138  ARMARX_CHECK_EQUAL(limits.size(), 3);
139  for (auto& pos : limits)
140  {
141  auto posVec = Split<float>(pos, ",", true);
142  ARMARX_CHECK_EQUAL(posVec.size(), 2);
143  croppingMin(i) = posVec.at(0);
144  croppingMax(i) = posVec.at(1);
145  i++;
146  }
147  }
148 
149  void
150  VoxelGridMappingProvider::process()
151  {
152 
153  typename pcl::PointCloud<PointType>::Ptr inputCloudPtr(new pcl::PointCloud<PointType>());
154  typename pcl::PointCloud<PointType>::Ptr downsampledTransformedInputCloudPtr(
155  new pcl::PointCloud<PointType>());
156  typename pcl::PointCloud<PointType>::Ptr outputCloudPtr(new pcl::PointCloud<PointType>());
157 
158  if (!collectingPointClouds)
159  {
160  ARMARX_DEBUG << deactivateSpam(10) << "Pointcloud processing is disabled";
161  usleep(10000);
162  return;
163  }
164  if (!waitForPointClouds(providerName, 10000))
165  {
166  ARMARX_INFO << "Timeout or error while waiting for point cloud data" << flush;
167  return;
168  }
169  else
170  {
171  getPointClouds(providerName, inputCloudPtr);
172  }
173  TIMING_START(FullCalc);
174 
175  ARMARX_INFO << "Got cloud of size: " << inputCloudPtr->size();
176  auto localRobot = robotPool->getRobot();
177  visionx::MetaPointCloudFormatPtr format = getPointCloudFormat(providerName);
178  ARMARX_INFO << deactivateSpam(1) << "pc timestamp: "
179  << IceUtil::Time::microSeconds(format->timeProvided).toDateTime();
180  if (robotStateComponent)
181  {
182  if (!RemoteRobot::synchronizeLocalCloneToTimestamp(
183  localRobot, robotStateComponent, format->timeProvided))
184  {
185  ARMARX_WARNING << deactivateSpam(5)
186  << "failed to sync robot to timestamp! conversion to globalpose "
187  "will fail! aborting";
188  return;
189  }
190  }
191 
192  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
193  // Eigen::Matrix4f sourceFrameGlobalPose = Eigen::Matrix4f::Identity();
194  if (robotStateComponent && sourceFrameName != "Global")
195  {
196  transform = localRobot->getRobotNode(sourceFrameName)->getGlobalPose();
197  }
198 
199  Eigen::Matrix4f cam2robot;
200  cam2robot << 0, 0, 1, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
201  // .. read or fill the source cloud
202  pcl::FrustumCulling<PointType> fc;
203  fc.setInputCloud(inputCloudPtr);
204  fc.setNegative(false);
205  fc.setVerticalFOV(35);
206  fc.setHorizontalFOV(45);
207  fc.setNearPlaneDistance(0.0);
208  fc.setFarPlaneDistance(3500);
209 
210  fc.setCameraPose(cam2robot);
211 
212  fc.filter(*outputCloudPtr);
213  ARMARX_INFO << "size before frustrum culling: " << inputCloudPtr->size()
214  << " size after: " << outputCloudPtr->size();
215  inputCloudPtr.swap(outputCloudPtr);
216 
217  ARMARX_INFO << "Robot camera transform is: " << transform;
218  // float N = 0.001f;
219  // transform (0,0) = transform (0,0) * N;
220  // transform (1,1) = transform (1,1) * N;
221  // transform (2,2) = transform (2,2) * N;
222  TIMING_START(PCl_TRANSFORM);
223  // Transform to intermediate cropping frame
224  pcl::transformPointCloud(*inputCloudPtr, *outputCloudPtr, transform);
225  outputCloudPtr.swap(inputCloudPtr);
226  TIMING_END(PCl_TRANSFORM);
227 
228  TIMING_START(PCL_FILTER);
229  // // assumes that z axis goes up in robot's root frame
230  pcl::PassThrough<PointType> pass;
231 
232 
233  pass.setInputCloud(inputCloudPtr);
234  pass.setFilterFieldName("z");
235  pass.setFilterLimits(croppingMin(2), croppingMax(2));
236  pass.filter(*outputCloudPtr);
237  outputCloudPtr.swap(inputCloudPtr);
238 
239  pass.setInputCloud(inputCloudPtr);
240  pass.setFilterFieldName("x");
241  pass.setFilterLimits(croppingMin(0), croppingMax(0));
242  pass.filter(*outputCloudPtr);
243  outputCloudPtr.swap(inputCloudPtr);
244 
245  pass.setInputCloud(inputCloudPtr);
246  pass.setFilterFieldName("y");
247  pass.setFilterLimits(croppingMin(1), croppingMax(1));
248  pass.filter(*outputCloudPtr);
249  outputCloudPtr.swap(inputCloudPtr);
250  TIMING_END(PCL_FILTER);
251 
252 
253  TIMING_START(PCl_DOWNSAMPLING);
254  gridLeafSize = getProperty<float>("GridSize");
255  pcl::VoxelGrid<PointType> grid;
256  grid.setMinimumPointsNumberPerVoxel(
257  getProperty<int>("MinimumPointNumberPerVoxel").getValue());
258  // grid.setDownsampleAllData(false);
259  grid.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
260  grid.setInputCloud(inputCloudPtr);
261  grid.filter(*outputCloudPtr);
262  ARMARX_VERBOSE << "Got cloud of size: " << inputCloudPtr->size()
263  << " after downsampling: " << outputCloudPtr->size();
264  outputCloudPtr.swap(inputCloudPtr);
265 
266 
267  if (!lastPointCloud)
268  {
269  lastPointCloud.reset(new Cloud());
270  }
271  // {
272  // *inputCloudPtr += *lastPointCloud;
273 
274  // pcl::VoxelGrid<PointType> grid;
275  // grid.setMinimumPointsNumberPerVoxel(2);
276  // // grid.setDownsampleAllData(false);
277  // grid.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
278  // grid.setInputCloud(inputCloudPtr);
279  // grid.filter(*outputCloudPtr);
280  // ARMARX_VERBOSE << "Cloud size after removing of temporal points (e.g. flickering): " << outputCloudPtr->size();
281  // outputCloudPtr.swap(inputCloudPtr);
282  // lastPointCloud = downsampledTransformedInputCloudPtr;
283  // }
284 
285 
286  TIMING_END(PCl_DOWNSAMPLING);
287 
288  // Eigen::Matrix4f transformScaleUp = Eigen::Matrix4f::Identity();
289  // float N2 = 1000.f;
290  // transformScaleUp (0,0) = transformScaleUp (0,0) * N2;
291  // transformScaleUp (1,1) = transformScaleUp (1,1) * N2;
292  // transformScaleUp (2,2) = transformScaleUp (2,2) * N2;
293 
294  // pcl::transformPointCloud(*inputCloudPtr, *outputCloudPtr, transformScaleUp);
295  // outputCloudPtr.swap(inputCloudPtr);
296 
297 
298  typename pcl::PointCloud<PointType>::Ptr occludedPoints(new pcl::PointCloud<PointType>());
299  typename pcl::PointCloud<PointType>::Ptr oldPointsInView(new pcl::PointCloud<PointType>());
300  {
301  TIMING_START(PCL_FrustumCulling);
302  std::unique_lock lock(dataMutex);
303  Eigen::Matrix4f cam2robot;
304  cam2robot << 0, 0, 1, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
305  // .. read or fill the source cloud
306  pcl::FrustumCulling<PointType> fc;
307  fc.setInputCloud(accumulatedPointCloud);
308  fc.setNegative(false);
309  fc.setVerticalFOV(40);
310  fc.setHorizontalFOV(50);
311  fc.setNearPlaneDistance(0.0);
312  fc.setFarPlaneDistance(2500);
313 
314  fc.setCameraPose(transform * cam2robot);
315 
316  fc.filter(*oldPointsInView);
317 
318 
319  fc.setNegative(true);
320  fc.filter(*outputCloudPtr);
321  outputCloudPtr.swap(accumulatedPointCloud);
322  TIMING_END(PCL_FrustumCulling);
323 
324 
325  TIMING_START(OCCLUSION);
326  Eigen::Vector3f cameraPos = transform.block<3, 1>(0, 3);
327  for (auto& p : *oldPointsInView)
328  {
329  Eigen::Vector3f currentPoint(
330  p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z);
331  VirtualRobot::MathTools::BaseLine<Eigen::Vector3f> line(transform.block<3, 1>(0, 3),
332  currentPoint - cameraPos);
333  float distP1 = (currentPoint - cameraPos).norm();
334  for (auto& p2pcl : *inputCloudPtr)
335  {
336  Eigen::Vector3f p2(
337  p2pcl._PointXYZRGBA::x, p2pcl._PointXYZRGBA::y, p2pcl._PointXYZRGBA::z);
338  float distP2 = (cameraPos - p2).norm();
339  if (distP1 > distP2)
340  {
341  auto distance = VirtualRobot::MathTools::distPointLine(line, p2);
342  ARMARX_CHECK_GREATER_EQUAL(distance, 0.0f);
343  if (distance < gridLeafSize)
344  {
345  auto newP = p;
346  // newP._PointXYZRGBA::r = 255;
347  // newP._PointXYZRGBA::g = 0;
348  // newP._PointXYZRGBA::b = 0;
349  occludedPoints->push_back(newP);
350  break;
351  }
352  }
353  }
354  }
355  TIMING_END(OCCLUSION);
356 
357  TIMING_START(RemoveRobotVoxels);
358  inputCloudPtr = removeRobotVoxels(inputCloudPtr, localRobot, gridLeafSize);
359  ARMARX_VERBOSE << "Size after removal of robot voxels: " << inputCloudPtr->size();
360  TIMING_END(RemoveRobotVoxels);
361 
362  TIMING_START(Accumulation);
363  // octtree->setInputCloud(inputCloudPtr);
364  // octtree->addPointsFromInputCloud();
365  // octtree->setInputCloud(occludedPoints);
366  // octtree->addPointsFromInputCloud();
367 
368  *accumulatedPointCloud += *inputCloudPtr;
369  *accumulatedPointCloud += *occludedPoints;
370 
371 
372  *accumulatedPointCloud += *occludedPoints;
373  ARMARX_VERBOSE << "Accumulated pc size: " << accumulatedPointCloud->size()
374  << " new point count: " << inputCloudPtr->size()
375  << " number of occluded points: " << occludedPoints->size();
376  TIMING_END(Accumulation);
377  TIMING_START(Downsampling2);
378  // filter again to remove duplicated points
379 
380  pcl::VoxelGrid<PointType> grid2;
381  grid2.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
382  grid2.setInputCloud(accumulatedPointCloud);
383  grid2.filter(*outputCloudPtr);
384  outputCloudPtr.swap(accumulatedPointCloud);
385  ARMARX_VERBOSE << "Accumulated pc size after grid filtering: "
386  << accumulatedPointCloud->size() << " " << VAROUT(gridLeafSize);
387  TIMING_END(Downsampling2);
388 
389 
390  provideResultPointClouds(accumulatedPointCloud);
391 
392  TIMING_START(PREPARATION);
393 
394  // std::map<Eigen::Vector3i, int> grid;
395  std::vector<VirtualRobot::VisualizationFactory::Color> gridColors;
396  gridPositions.reserve(accumulatedPointCloud->size());
397  gridColors.reserve(accumulatedPointCloud->size());
398 
399  DebugDrawerTriMesh ddTriMesh;
400 
401  this->gridPositions.clear();
402  this->gridPositions.reserve(accumulatedPointCloud->size());
403  // pcl::octree::OctreePointCloud<PointType>::AlignedPointTVector points;
404  // octtree->getOccupiedVoxelCenters(points);
405  for (auto& p : *accumulatedPointCloud)
406  {
407  if (!std::isnan(p._PointXYZRGBA::x) && !std::isnan(p._PointXYZRGBA::y) &&
408  !std::isnan(p._PointXYZRGBA::z))
409  {
410 
411  this->gridPositions.push_back(
412  {p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z});
413  gridColors.push_back(
414  VirtualRobot::VisualizationFactory::Color(p._PointXYZRGBA::r / 255.f,
415  p._PointXYZRGBA::g / 255.f,
416  p._PointXYZRGBA::b / 255.f,
417  0.8));
418  }
419  }
420  TIMING_END(PREPARATION);
421 
422  std::vector<Eigen::Vector3f> gridPositionsEigen = armarx::transform(
423  this->gridPositions,
424  +[](const Vector3f& p) -> Eigen::Vector3f {
425  return {p.e0, p.e1, p.e2};
426  });
427 
428 
429  ARMARX_INFO << VAROUT(gridPositionsEigen.size());
430 
431  TIMING_START(MESHING);
432 
433  gridMesh = VirtualRobot::TriMeshUtils::CreateSparseBoxGrid(
434  Eigen::Matrix4f::Identity(),
435  gridPositionsEigen,
436  gridLeafSize,
437  gridLeafSize,
438  gridLeafSize,
439  VirtualRobot::VisualizationFactory::Color::Blue(),
440  gridColors);
441  ddTriMesh = DebugDrawerUtils::convertTriMesh(*gridMesh);
442  TIMING_END(MESHING);
443  TIMING_START(VISU);
444  // DebugDrawer24BitColoredPointCloud ddPC;
445  // visionx::tools::convertFromPCLToDebugDrawer(accumulatedPointCloud, ddPC, visionx::tools::eConvertAsColors);
446  // dd->set24BitColoredPointCloudVisu("VoxelGrid", "PointCloud", ddPC);
447  // ARMARX_INFO << "Sending point cloud to debug drawer - size: " << ddTriMesh.faces.size() << " first: " << ddTriMesh.vertices.at(0).x << ", " << ddTriMesh.vertices.at(0).y << ", " << ddTriMesh.vertices.at(0).z;
448  dd->setTriMeshVisu("VoxelGrid", "PointCloudCollisionGrid", ddTriMesh);
449  TIMING_END(VISU);
450  }
451 
452 
453  if (accumulatedPointCloud->size() > 0 && skipper->checkFrequency("SavingPCD") &&
454  !getProperty<std::string>("PointCloudStoreFilepath").getValue().empty())
455  {
456  std::unique_lock lock(dataMutex);
457  ARMARX_INFO << "Saving point cloud to "
458  << getProperty<std::string>("PointCloudStoreFilepath").getValue();
459  pcl::io::savePCDFile(getProperty<std::string>("PointCloudStoreFilepath").getValue(),
460  *accumulatedPointCloud);
461  }
462  TIMING_END(FullCalc);
463 
464 
465  cycleKeeper->waitForCycleDuration();
466  }
467 
468  VoxelGridMappingProvider::CloudPtr
469  VoxelGridMappingProvider::removeRobotVoxels(const VoxelGridMappingProvider::CloudPtr& cloud,
470  const VirtualRobot::RobotPtr& robot,
471  float distanceThreshold)
472  {
473  CloudPtr result(new Cloud());
474  VirtualRobot::CDManager cdm;
475  auto colModels = robot->getCollisionModels();
476  auto collisionChecker = robot->getCollisionChecker();
477  for (auto& p : *cloud)
478  {
479  bool collisionFound = false;
480  for (auto& colModel : colModels)
481  {
482  Eigen::Vector3f pEigen(p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z);
483  if (collisionChecker->checkCollision(colModel, pEigen, distanceThreshold))
484  {
485  collisionFound = true;
486  break;
487  }
488  }
489  if (!collisionFound)
490  {
491  result->push_back(p);
492  }
493  }
494  return result;
495  }
496 
497  Vector3fSeq
498  VoxelGridMappingProvider::getFilledGridPositions(const Ice::Current&) const
499  {
500  std::unique_lock lock(dataMutex);
501  return gridPositions;
502  }
503 
504  Ice::Float
505  VoxelGridMappingProvider::getGridSize(const Ice::Current&) const
506  {
507  return gridLeafSize;
508  }
509 
510  void
511  VoxelGridMappingProvider::startCollectingPointClouds(const Ice::Current&)
512  {
513  collectingPointClouds = true;
514  }
515 
516  void
517  VoxelGridMappingProvider::stopCollectingPointClouds(const Ice::Current&)
518  {
519  collectingPointClouds = false;
520  }
521 
522  void
523  VoxelGridMappingProvider::reset(const Ice::Current&)
524  {
525  std::unique_lock lock(dataMutex);
526  ARMARX_INFO << "Clearing voxel grid";
527  accumulatedPointCloud->clear();
528  }
529 
530  void
531  VoxelGridMappingProvider::componentPropertiesUpdated(
532  const std::set<std::string>& changedProperties)
533  {
534  if (changedProperties.count("BoundingBox"))
535  {
536  updateBoundaries();
537  }
538  }
539 } // namespace visionx
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:289
algorithm.h
visionx::VoxelGridMappingProviderPropertyDefinitions
Definition: VoxelGridMappingProvider.h:53
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:919
TIMING_END
#define TIMING_END(name)
Definition: TimeUtil.h:306
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
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:512
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:36
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
visionx::VoxelGridMappingProvider::Cloud
pcl::PointCloud< PointType > Cloud
Definition: VoxelGridMappingProvider.h:125
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:234
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::RobotPool
This class holds a pool of local VirtualRobots for multi threaded applications that can be requested ...
Definition: RobotPool.h:40
IceReportSkipper.h
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
PointCloudConversions.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:566
armarx::DebugDrawerUtils::convertTriMesh
static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel &trimesh)
Definition: DebugDrawerUtils.cpp:6
StringHelperTemplates.h
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:95
visionx::VoxelGridMappingProvider::CloudPtr
Cloud::Ptr CloudPtr
Definition: VoxelGridMappingProvider.h:126
DebugDrawerUtils.h
armarx::IceReportSkipper
Definition: IceReportSkipper.h:36
VoxelGridMappingProvider.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::view_selection::skills::localRobot
VirtualRobot::RobotPtr localRobot
Definition: LookForObjectsWithKinematicUnit.cpp:25
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:35
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
norm
double norm(const Point &a)
Definition: point.hpp:102