OpenNIPointCloudProvider.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 VisionX::ArmarXObjects::OpenNIPointCloudProvider
19  * @author David Schiebener (schiebener at kit dot edu)
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
26 
27 #include <pcl/io/openni2/openni2_device_manager.h>
28 #include <pcl/io/pcd_io.h>
29 
32 
35 
36 #include <Calibration/Calibration.h>
37 #include <Image/ImageProcessor.h>
38 
39 
40 using namespace armarx;
41 using namespace pcl;
42 using namespace cv;
43 
44 #if PCL_VERSION > PCL_VERSION_CALC(1, 12, 0)
45 template <typename T>
46 using FctT = std::function<T>;
47 #else
48 template <typename T>
49 using FctT = boost::function<T>;
50 #endif
51 
52 namespace visionx
53 {
54  void
55  OpenNIPointCloudProvider::onInitComponent()
56  {
58  ARMARX_VERBOSE << "onInitComponent()";
59 
60  offeringTopicFromProperty("RobotHealthTopicName");
61  offeringTopicFromProperty("DebugObserverName");
62 
63  ImageProvider::onInitComponent();
64  CapturingPointCloudProvider::onInitComponent();
65  }
66 
67  void
68  OpenNIPointCloudProvider::onConnectComponent()
69  {
71  ARMARX_VERBOSE << "onConnectComponent()";
72 
73  getTopicFromProperty(debugObserver, "DebugObserverName");
74  getTopicFromProperty(robotHealthTopic, "RobotHealthTopicName");
75 
76  ImageProvider::onConnectComponent();
77  CapturingPointCloudProvider::onConnectComponent();
78  }
79 
80  void
81  OpenNIPointCloudProvider::onDisconnectComponent()
82  {
84  // hack to prevent deadlock
85  captureEnabled = false;
86  ImageProvider::onDisconnectComponent();
87  CapturingPointCloudProvider::onDisconnectComponent();
88  }
89 
90  void
91  OpenNIPointCloudProvider::onExitComponent()
92  {
94  ARMARX_VERBOSE << "onExitComponent()";
95 
96  ImageProvider::onExitComponent();
97  CapturingPointCloudProvider::onExitComponent();
98  ARMARX_INFO << "Resetting grabber interface";
99  grabberInterface.reset();
100  }
101 
102  bool
103  OpenNIPointCloudProvider::loadCalibrationFile(std::string fileName,
104  visionx::CameraParameters& calibration)
105  {
106  ARMARX_TRACE;
107  ArmarXDataPath::getAbsolutePath(fileName, fileName);
108  FileStorage fs(fileName, FileStorage::READ);
109 
110  if (!fs.isOpened())
111  {
112  ARMARX_WARNING << "unable to read calibration file " << fileName;
113  return false;
114  }
115 
116  std::string cameraName = fs["camera_name"];
117  ARMARX_LOG << "loading calibration file for " << cameraName;
118 
119 
120  Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
121  int imageWidth = fs["image_width"];
122  int imageHeight = fs["image_height"];
123 
124  if (imageWidth != dimensions(0) || imageHeight != dimensions(1))
125  {
126  ARMARX_ERROR << "invalid camera size";
127  return false;
128  }
129 
130  Mat cameraMatrix, distCoeffs;
131 
132  fs["camera_matrix"] >> cameraMatrix;
133  fs["distortion_coefficients"] >> distCoeffs;
134 
135  for (int i = 0; i < 5; i++)
136  {
137  calibration.distortion[i] = distCoeffs.at<float>(0, i);
138  }
139 
140  calibration.focalLength[0] = cameraMatrix.at<float>(0, 0);
141  calibration.focalLength[1] = cameraMatrix.at<float>(1, 1);
142 
143  calibration.principalPoint[0] = cameraMatrix.at<float>(0, 2);
144  calibration.principalPoint[1] = cameraMatrix.at<float>(1, 2);
145 
146  return true;
147  }
148 
149  void
150  OpenNIPointCloudProvider::onInitCapturingPointCloudProvider()
151  {
152  ARMARX_TRACE;
153  ARMARX_VERBOSE << "onInitCapturingPointCloudProvider()";
154 
155 
156  Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
157 
158  width = dimensions(0);
159  height = dimensions(1);
160 
161  newPointCloudCapturingRequested = true;
162  newImageCapturingRequested = true;
163 
164  ARMARX_VERBOSE << "creating OpenNI2Grabber " << width << "x" << height << "@" << frameRate
165  << " fps";
166  std::string configuredDeviceId = getProperty<std::string>("OpenNIDeviceId").getValue();
167  using namespace pcl::io::openni2;
168  auto connectedDeviceURIs = *OpenNI2DeviceManager::getInstance()->getConnectedDeviceURIs();
169  ARMARX_INFO << "connected devices: " << connectedDeviceURIs;
170  if (!configuredDeviceId.empty() && configuredDeviceId.size() > 2)
171  {
172  ARMARX_TRACE;
173  size_t i = 1;
174  std::string deviceIndexNumber;
175  ARMARX_INFO << "Trying to find device with configured URI: '" << configuredDeviceId
176  << "'";
177  for (auto& uri : connectedDeviceURIs)
178  {
179  if (uri == configuredDeviceId)
180  {
181  ARMARX_INFO << "Found device with URI " << configuredDeviceId << " at index "
182  << i;
183  deviceIndexNumber = "#" + std::to_string(i);
184  break;
185  }
186  i++;
187  }
188  if (deviceIndexNumber.empty())
189  {
190  ARMARX_WARNING << "Could not find device with URI: " << configuredDeviceId
191  << " - Connecting now to any device";
192  grabberInterface.reset(new pcl::io::OpenNI2Grabber());
193  }
194  else
195  {
196  grabberInterface.reset(new pcl::io::OpenNI2Grabber(deviceIndexNumber));
197  }
198  }
199  else if (!configuredDeviceId.empty())
200  {
201  ARMARX_TRACE;
202  ARMARX_INFO << "Connecting to device id " << configuredDeviceId;
203  grabberInterface.reset(new pcl::io::OpenNI2Grabber("#" + configuredDeviceId));
204  }
205  else
206  {
207  ARMARX_TRACE;
208  grabberInterface.reset(new pcl::io::OpenNI2Grabber());
209  }
210  ARMARX_TRACE;
211 
212  visionx::CameraParameters RGBCameraIntrinsics;
213  RGBCameraIntrinsics.focalLength.resize(2, 0.0f);
214  RGBCameraIntrinsics.principalPoint.resize(2, 0.0f);
215  RGBCameraIntrinsics.distortion.resize(4, 0.0f);
216 
217  //Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
218  RGBCameraIntrinsics.rotation =
220  RGBCameraIntrinsics.translation =
221  visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
222 
223  ARMARX_TRACE;
224  std::string rgbCameraCalibrationFile =
225  getProperty<std::string>("RGBCameraCalibrationFile").getValue();
226  if (!rgbCameraCalibrationFile.empty() &&
227  loadCalibrationFile(rgbCameraCalibrationFile, RGBCameraIntrinsics))
228  {
229  ARMARX_TRACE;
230  grabberInterface->setRGBCameraIntrinsics(RGBCameraIntrinsics.focalLength[0],
231  RGBCameraIntrinsics.focalLength[1],
232  RGBCameraIntrinsics.principalPoint[0],
233  RGBCameraIntrinsics.principalPoint[1]);
234  }
235  else
236  {
237  ARMARX_TRACE;
238  ARMARX_INFO << "unable to load calibration file. using default values.";
239  double fx, fy, cx, cy;
240  grabberInterface->getRGBCameraIntrinsics(fx, fy, cx, cy);
241 
242  if (std::isnan(fx))
243  {
244  if (grabberInterface->getDevice()->hasColorSensor())
245  {
246  fx = fy = grabberInterface->getDevice()->getColorFocalLength();
247  }
248 
249  cx = (width - 1) / 2.0;
250  cy = (height - 1) / 2.0;
251  }
252  ARMARX_INFO << "rgb camera fx: " << fx << " fy: " << fy << " cx: " << cx
253  << " cy: " << cy;
254 
255  RGBCameraIntrinsics.principalPoint[0] = cx;
256  RGBCameraIntrinsics.principalPoint[1] = cy;
257  RGBCameraIntrinsics.focalLength[0] = fx;
258  RGBCameraIntrinsics.focalLength[1] = fy;
259  RGBCameraIntrinsics.width = width;
260  RGBCameraIntrinsics.height = height;
261  }
262 
263  visionx::CameraParameters depthCameraIntrinsics;
264  depthCameraIntrinsics.focalLength.resize(2, 0.0f);
265  depthCameraIntrinsics.principalPoint.resize(2, 0.0f);
266  depthCameraIntrinsics.distortion.resize(4, 0.0f);
267  depthCameraIntrinsics.rotation =
269  depthCameraIntrinsics.translation =
270  visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
271 
272  std::string depthCameraCalibrationFile =
273  getProperty<std::string>("depthCameraCalibrationFile").getValue();
274  if (!depthCameraCalibrationFile.empty() &&
275  loadCalibrationFile(depthCameraCalibrationFile, depthCameraIntrinsics))
276  {
277  ARMARX_TRACE;
278  grabberInterface->setDepthCameraIntrinsics(depthCameraIntrinsics.focalLength[0],
279  depthCameraIntrinsics.focalLength[1],
280  depthCameraIntrinsics.principalPoint[0],
281  depthCameraIntrinsics.principalPoint[1]);
282  }
283  else
284  {
285  ARMARX_TRACE;
286  ARMARX_INFO << "unable to load calibration file. using default values.";
287  double fx, fy, cx, cy;
288  grabberInterface->getDepthCameraIntrinsics(fx, fy, cx, cy);
289 
290  if (std::isnan(fx))
291  {
292  if (grabberInterface->getDevice()->hasDepthSensor())
293  {
294  fx = fy = grabberInterface->getDevice()->getDepthFocalLength();
295  }
296  cx = (width - 1) / 2.0;
297  cy = (height - 1) / 2.0;
298  }
299  ARMARX_INFO << "depth camera fx: " << fx << " fy: " << fy << " cx: " << cx
300  << " cy: " << cy;
301 
302  depthCameraIntrinsics.principalPoint[0] = cx;
303  depthCameraIntrinsics.principalPoint[1] = cy;
304  depthCameraIntrinsics.focalLength[0] = fx;
305  depthCameraIntrinsics.focalLength[1] = fy;
306  depthCameraIntrinsics.width = width;
307  depthCameraIntrinsics.height = height;
308  }
309 
310  ARMARX_TRACE;
311 
312  calibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
313  calibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
314  calibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
315  calibration.calibrationRight.cameraParam = depthCameraIntrinsics;
316  calibration.rectificationHomographyLeft =
317  visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
318  calibration.rectificationHomographyRight =
319  visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
320 
321  ARMARX_INFO << "Baseline:" << grabberInterface->getDevice()->getBaseline();
322 
323  pointcloudBuffer = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(
324  new pcl::PointCloud<pcl::PointXYZRGBA>(width, height));
325 
326  rgbImages = new CByteImage*[2];
327 
328  rgbImages[0] = new CByteImage(width, height, CByteImage::eRGB24);
329  rgbImages[1] = new CByteImage(width, height, CByteImage::eRGB24);
330  ::ImageProcessor::Zero(rgbImages[0]);
331  ::ImageProcessor::Zero(rgbImages[1]);
332 
333  if (grabberInterface->getDevice()->hasColorSensor())
334  {
335  FctT<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> pointCloudCallback =
336  [this](auto&& PH1)
337  {
338  callbackFunctionForOpenNIGrabberPointCloudWithRGB(std::forward<decltype(PH1)>(PH1));
339  };
340  grabberInterface->registerCallback(pointCloudCallback);
341 
342  FctT<void(const pcl::io::Image::Ptr&, const pcl::io::DepthImage::Ptr&, float)>
343  imageCallback = [this](auto&& PH1, auto&& PH2, auto&& PH3)
344  {
345  grabImages(std::forward<decltype(PH1)>(PH1),
346  std::forward<decltype(PH2)>(PH2),
347  std::forward<decltype(PH3)>(PH3));
348  };
349  grabberInterface->registerCallback(imageCallback);
350  }
351  else
352  {
353  ARMARX_INFO << "depth sensor. no color image available.";
354 
355  FctT<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> pointCloudCallback =
356  [this](auto&& PH1)
357  { callbackFunctionForOpenNIGrabberPointCloud(std::forward<decltype(PH1)>(PH1)); };
358  grabberInterface->registerCallback(pointCloudCallback);
359 
360  FctT<void(const pcl::io::IRImage::Ptr&, const pcl::io::DepthImage::Ptr&, float)>
361  imageCallback = [this](auto&& PH1, auto&& PH2, auto&& PH3)
362  {
363  grabDepthImage(std::forward<decltype(PH1)>(PH1),
364  std::forward<decltype(PH2)>(PH2),
365  std::forward<decltype(PH3)>(PH3));
366  };
367  grabberInterface->registerCallback(imageCallback);
368  }
369  }
370 
371  void
372  OpenNIPointCloudProvider::onConnectPointCloudProvider()
373  {
374  ARMARX_CHECK_NOT_NULL(heartbeatPlugin);
375  heartbeatPlugin->signUp(armarx::Duration::MilliSeconds(500),
377  {"Vision", "Camera"},
378  "OpenNIPointCloudProvider");
379  }
380 
381  void
382  OpenNIPointCloudProvider::onStartCapture(float frameRate)
383  {
384  ARMARX_INFO << "Openni grabber interface name: " << grabberInterface->getName();
385  ARMARX_INFO << "Openni device interface name: " << grabberInterface->getDevice()->getName()
386  << " id: " << grabberInterface->getDevice()->getStringID();
387 
388 
389  grabberInterface->getDevice()->setAutoExposure(
390  getProperty<bool>("EnableAutoExposure").getValue());
391  grabberInterface->getDevice()->setAutoWhiteBalance(
392  getProperty<bool>("EnableAutoWhiteBalance").getValue());
393 
394  grabberInterface->start();
395 
396  lastReportTimestamp = TimeUtil::GetTime();
397  }
398 
399  void
400  OpenNIPointCloudProvider::onStopCapture()
401  {
402  ARMARX_TRACE;
403  ARMARX_INFO << "Stopping OpenNI Grabber Interface";
404  if (grabberInterface)
405  {
406  grabberInterface->stop();
407  }
408  ARMARX_INFO << "Stopped OpenNI Grabber Interface";
409 
410  std::scoped_lock lock(syncMutex);
411 
412  condition.notify_one();
413  }
414 
415  void
416  OpenNIPointCloudProvider::onExitCapturingPointCloudProvider()
417  {
418  ARMARX_TRACE;
419  ARMARX_VERBOSE << "onExitCapturingPointCloudProvider()";
420 
421  delete rgbImages[0];
422  delete rgbImages[1];
423 
424  delete[] rgbImages;
425  }
426 
427  void
428  OpenNIPointCloudProvider::onInitImageProvider()
429  {
430  ARMARX_TRACE;
431  Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
432  setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
433  setNumberImages(2);
434  }
435 
436  StereoCalibration
437  OpenNIPointCloudProvider::getStereoCalibration(const Ice::Current&)
438  {
439  return calibration;
440  }
441 
442  void
443  OpenNIPointCloudProvider::callbackFunctionForOpenNIGrabberPointCloudWithRGB(
444  const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& newCloud)
445  {
446  ARMARX_TRACE;
447  ARMARX_INFO << deactivateSpam(1e10) << "got first pointcloud";
448  if (newPointCloudCapturingRequested)
449  {
450  long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
451  timeOfLastPointCloudCapture = IceUtil::Time::now().toMicroSeconds() - offset;
452  pcl::copyPointCloud(*newCloud, *pointcloudBuffer);
453  std::scoped_lock lock(syncMutex);
454  auto size = newCloud->size();
455  pointcloudBuffer->resize(size);
456  float scaling = 1000.f;
457  for (size_t i = 0; i < size; i++)
458  {
459  pointcloudBuffer->at(i).x = newCloud->at(i).x * scaling;
460  pointcloudBuffer->at(i).y = newCloud->at(i).y * scaling;
461  pointcloudBuffer->at(i).z = newCloud->at(i).z * scaling;
462  }
463 
464  newPointCloudCapturingRequested = false;
465 
466  condition.notify_one();
467  }
468  }
469 
470  void
471  OpenNIPointCloudProvider::callbackFunctionForOpenNIGrabberPointCloud(
472  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& newCloud)
473  {
474  ARMARX_TRACE;
475  ARMARX_INFO << deactivateSpam(1e10) << "got first pointcloud";
476 
477  if (newPointCloudCapturingRequested)
478  {
479 
480  long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
481  timeOfLastPointCloudCapture = IceUtil::Time::now().toMicroSeconds() - offset;
482  Eigen::Affine3f transform(Eigen::Scaling(1000.0f));
483 
484  std::scoped_lock lock(syncMutex);
485  pcl::copyPointCloud(*newCloud, *pointcloudBuffer);
486  pcl::transformPointCloud(*pointcloudBuffer, *pointcloudBuffer, transform);
487 
488 
489  newPointCloudCapturingRequested = false;
490 
491  condition.notify_one();
492  }
493  }
494 
495  void
496  OpenNIPointCloudProvider::grabDepthImage(const pcl::io::IRImage::Ptr& IRImage,
497  const pcl::io::DepthImage::Ptr& depthImage,
498  float x)
499  {
500  ARMARX_TRACE;
501  ARMARX_INFO << deactivateSpam(1e10) << "got first depthImage";
502 
503  if (newImageCapturingRequested)
504  {
505  long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
506  timeOfLastImageCapture = IceUtil::Time::now().toMicroSeconds() - offset;
507  cv::Mat depthBuffer(height, width, CV_16UC1);
508  depthImage->fillDepthImageRaw(width, height, (unsigned short*)depthBuffer.data);
509 
510  std::scoped_lock lock(syncMutex);
511 
512  // boost::chrono::high_resolution_clock::time_point timestamp = depthImage->getSystemTimestamp(); <-- wrong timestamp!
513 
514  for (int j = 0; j < depthBuffer.rows; j++)
515  {
516  int index = 3 * (j * width);
517  for (int i = 0; i < depthBuffer.cols; i++)
518  {
519  unsigned short value = depthBuffer.at<unsigned short>(j, i);
521  rgbImages[1]->pixels[index + 0],
522  rgbImages[1]->pixels[index + 1],
523  rgbImages[1]->pixels[index + 2],
524  false);
525  // rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
526  // rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
527  index += 3;
528  }
529  }
530 
531 
532  newImageCapturingRequested = false;
533 
534  condition.notify_one();
535  }
536  }
537 
538  void
539  OpenNIPointCloudProvider::grabImages(const pcl::io::Image::Ptr& RGBImage,
540  const pcl::io::DepthImage::Ptr& depthImage,
541  float reciprocalFocalLength)
542  {
543  ARMARX_TRACE;
544  ARMARX_INFO << deactivateSpam(1e10) << "got first image";
545 
546  if (newImageCapturingRequested)
547  {
548  long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
549  timeOfLastImageCapture = IceUtil::Time::now().toMicroSeconds() - offset;
550 
551  cv::Mat depthBuffer(height, width, CV_16UC1);
552  depthImage->fillDepthImageRaw(width, height, (unsigned short*)depthBuffer.data);
553 
554  std::scoped_lock lock(syncMutex);
555 
556  // boost::chrono::high_resolution_clock::time_point timestamp = depthImage->getSystemTimestamp(); <-- wrong timestamp!
557 
558 
559  for (int j = 0; j < depthBuffer.rows; j++)
560  {
561  int index = 3 * (j * width);
562  for (int i = 0; i < depthBuffer.cols; i++)
563  {
564  unsigned short value = depthBuffer.at<unsigned short>(j, i);
566  rgbImages[1]->pixels[index + 0],
567  rgbImages[1]->pixels[index + 1],
568  rgbImages[1]->pixels[index + 2],
569  false);
570  // rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
571  // rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
572  index += 3;
573  }
574  }
575 
576  /*
577 
578 
579  cv::Mat depthBuffer = cv::Mat(height, width, CV_32FC1);
580  depthImage->fillDisparityImage(width, height, (float*) depthBuffer.data);
581  boost::mutex::scoped_lock lock(captureMutex);
582 
583  calibration.calibrationRight.cameraParam.translation[0] = depthImage->getBaseline();
584 
585  for (int j = 0; j < depthBuffer.rows; j++)
586  {
587  for (int i = 0; i < depthBuffer.cols; i++)
588  {
589  int value = static_cast<int>(depthBuffer.at<float>(j, i));
590  rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
591  rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
592  rgbImages[1]->pixels[3 * (j * width + i) + 2] = (value >> 16) & 0xFF;
593  }
594  } for (int j = 0; j < depthBuffer.rows; j++)
595  {
596  for (int i = 0; i < depthBuffer.cols; i++)
597  {
598  unsigned short value = depthBuffer.at<unsigned short>(j, i);
599 
600  rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
601  rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
602  }
603  }
604 
605  */
606 
607  RGBImage->fillRGB(width, height, rgbImages[0]->pixels);
608 
609  newImageCapturingRequested = false;
610 
611  condition.notify_one();
612  }
613  }
614 
615  bool
616  OpenNIPointCloudProvider::doCapture()
617  {
618  ARMARX_TRACE;
619  std::unique_lock lock(syncMutex);
620 
621  newPointCloudCapturingRequested = true;
622  newImageCapturingRequested = true;
623 
624  while (captureEnabled && (newPointCloudCapturingRequested || newImageCapturingRequested))
625  {
626  // condition.wait(lock);
627  condition.wait_for(lock, std::chrono::milliseconds(100));
628  }
629 
630  if (!captureEnabled)
631  {
632  return false;
633  }
634 
635  // provide images
636  provideImages(rgbImages, IceUtil::Time::microSeconds(timeOfLastImageCapture));
637  // provide pointclpoud
638  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud(
639  new pcl::PointCloud<PointXYZRGBA>(*pointcloudBuffer));
640  // pointcloud->header.stamp = pointcloudBuffer->header.stamp; <<- wrong timestamp!
641  pointcloud->header.stamp = timeOfLastPointCloudCapture;
642  providePointCloud(pointcloud);
643 
644  heartbeatPlugin->heartbeat();
645 
647  IceUtil::Time reportTime = now - lastReportTimestamp;
648 
649  StringVariantBaseMap durations;
650  durations["report_time_ms"] = new armarx::Variant(reportTime.toMilliSecondsDouble());
651  debugObserver->setDebugChannel(getName(), durations);
652 
653 
654  lastReportTimestamp = now;
655 
656  return true;
657  }
658 
659  std::vector<imrec::ChannelPreferences>
660  OpenNIPointCloudProvider::getImageRecordingChannelPreferences(const Ice::Current&)
661  {
662  ARMARX_TRACE;
663 
664  imrec::ChannelPreferences rgb;
665  rgb.requiresLossless = false;
666  rgb.name = "rgb";
667 
668  imrec::ChannelPreferences depth;
669  depth.requiresLossless = true;
670  depth.name = "depth";
671 
672  return {rgb, depth};
673  }
674 
676  OpenNIPointCloudProvider::createPropertyDefinitions()
677  {
678  return PropertyDefinitionsPtr(
679  new OpenNIPointCloudProviderPropertyDefinitions(getConfigIdentifier()));
680  }
681 
682  OpenNIPointCloudProvider::OpenNIPointCloudProvider()
683  {
684  addPlugin(heartbeatPlugin);
685  }
686 } // namespace visionx
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
pcl
Definition: pcl_point_operators.cpp:4
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
index
uint8_t index
Definition: EtherCATFrame.h:59
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
FctT
boost::function< T > FctT
Definition: OpenNIPointCloudProvider.cpp:49
OpenNIPointCloudProvider.h
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
visionx::tools::convertEigenVecToVisionX
visionx::types::Vec convertEigenVecToVisionX(Eigen::VectorXf v)
Definition: TypeMapping.cpp:655
visionx::OpenNIPointCloudProviderPropertyDefinitions
Definition: OpenNIPointCloudProvider.h:62
visionx::tools::depthValueToRGB
void depthValueToRGB(unsigned int depthInMM, unsigned char &r, unsigned char &g, unsigned char &b, bool noiseResistant=false)
Definition: ImageUtil.h:176
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:163
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::tools::convertEigenMatToVisionX
visionx::types::Mat convertEigenMatToVisionX(Eigen::MatrixXf m)
Definition: TypeMapping.cpp:677
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:315
ImageUtil.h
cv
Definition: helper.h:35
TypeMapping.h
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
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
Variant.h
visionx::tools::createDefaultMonocularCalibration
MonocularCalibration createDefaultMonocularCalibration()
Creates a MonocularCalibration with all parameters set to a neutral value.
Definition: ImageUtil.cpp:237
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:55