29 #include <opencv2/opencv.hpp>
32 #include <Image/ImageProcessor.h>
51 std::string ImageProcessor::ListenerSuffix =
".ImageListener";
52 void ImageProcessor::onInitComponent()
54 if (hasProperty(
"CompressionType") && hasProperty(
"CompressionQuality"))
56 setCompressionType(getProperty<CompressionType>(
"CompressionType").getValue(), getProperty<int>(
"CompressionQuality").getValue());
59 onInitImageProcessor();
60 float cycleTime = desiredFps == 0 ? 0 : 1000.f / desiredFps;
64 processorTask->setDelayWarningTolerance(1000);
68 processorTask->setDelayWarningTolerance(cycleTime * 5);
72 void ImageProcessor::onConnectComponent()
74 onConnectImageProcessor();
77 if (startProcessorTask)
79 processorTask->start();
83 void ImageProcessor::onDisconnectComponent()
85 processorTask->stop();
86 onDisconnectImageProcessor();
89 void ImageProcessor::onExitComponent()
92 onExitImageProcessor();
100 getConfigIdentifier()));
103 void ImageProcessor::runProcessor()
112 std::string ImageProcessor::getImageListenerTopicName(std::string providerName)
const
114 return providerName + ListenerSuffix;
117 void ImageProcessor::usingImageProvider(std::string providerName)
121 usingTopic(getImageListenerTopicName(providerName));
125 if (hasProperty(
"ForceIceTransfer") && getProperty<bool>(
"ForceIceTransfer").getValue())
127 consumer->setTransferMode(armarx::eIce);
130 std::pair<std::string, armarx::IceSharedMemoryConsumer<unsigned char>::pointer_type> consumer_info;
131 consumer_info.first = providerName;
132 consumer_info.second = consumer;
134 usedImageProviders.insert(consumer_info);
137 void ImageProcessor::releaseImageProvider(std::string providerName)
140 auto it = usedImageProviders.find(providerName);
141 if (it != usedImageProviders.end())
143 removeProxyDependency(providerName);
145 removeProxyDependency(consumer->getMemoryName());
146 unsubscribeFromTopic(getImageListenerTopicName(providerName));
147 imageProviderInfoMap.erase(providerName);
148 usedImageProviders.erase(it);
152 ImageProviderInfo ImageProcessor::getImageProvider(std::string providerName, ImageType destinationImageType,
bool waitForProxy)
157 std::unique_lock lock(imageProviderInfoMutex);
163 provider_info.
proxy = getProxy<ImageProviderInterfacePrx>(providerName, waitForProxy);
177 std::pair<std::string, ImageProviderInfo> entry(providerName, provider_info);
178 imageProviderInfoMap.insert(entry);
181 ImageProviderMap::iterator iter = usedImageProviders.find(providerName);
183 if (iter == usedImageProviders.end())
185 usingImageProvider(providerName);
188 if (!provider_info.
proxy->hasSharedMemorySupport())
190 ARMARX_INFO <<
"shared memory not available for provider " << providerName;
191 usedImageProviders[providerName]->setTransferMode(eIce);
192 imageProviderInfoMap[providerName].imageTransferMode = eIceTransfer;
193 imageProviderInfoMap[providerName].info =
new MetaInfoSizeBase(0, 0,
TimeUtil::GetTime().toMicroSeconds());
194 removeProxyDependency(usedImageProviders[providerName]->getMemoryName());
201 usedImageProviders[providerName]->start();
203 auto transferMode = usedImageProviders[providerName]->getTransferMode();
204 switch (transferMode)
207 imageProviderInfoMap[providerName].imageTransferMode = eIceTransfer;
210 imageProviderInfoMap[providerName].imageTransferMode = eSharedMemoryTransfer;
215 std::unique_lock lock2(statisticsMutex);
216 statistics[providerName].pollingFPS.reset();
217 statistics[providerName].imageProviderFPS.reset();
219 return provider_info;
222 ImageProviderInfo ImageProcessor::getImageProvider(std::string name,
bool waitForProxy, ImageType destinationImageType)
224 return getImageProvider(name, destinationImageType, waitForProxy);
227 void ImageProcessor::enableResultImages(
int numberImages, ImageDimension imageDimension, ImageType imageType,
const std::string& name)
229 if (!resultImageProvider)
231 ARMARX_VERBOSE <<
"Enabling ResultImageProvider with " << numberImages <<
" result images.";
232 resultImageProvider = Component::create<ResultImageProvider>();
233 resultImageProvider->setName(name.empty() ? getName() +
"Result" : name);
235 getArmarXManager()->addObject(resultImageProvider);
237 resultImageProvider->setNumberResultImages(numberImages);
238 resultImageProvider->setResultImageFormat(imageDimension, imageType);
241 resultImageProvider->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
245 void ImageProcessor::provideResultImages(CByteImage** images, armarx::MetaInfoSizeBasePtr info)
247 if (resultImageProvider)
251 resultImageProvider->provideResultImages(images, info->timeProvided);
260 void ImageProcessor::provideResultImages(
const std::vector<CByteImageUPtr >& images, MetaInfoSizeBasePtr info)
262 if (resultImageProvider)
266 resultImageProvider->provideResultImages(images, info->timeProvided);
275 bool ImageProcessor::waitForImages(
int milliseconds)
277 if (imageProviderInfoMap.size() == 0)
279 ARMARX_ERROR <<
"Calling getImages without ImageProvider name, and no ImageProvider is available - did you forget to call useImageProvider?";
282 else if (imageProviderInfoMap.size() > 1)
284 std::string outputText =
"Several image providers are available - using the first one in the list:";
285 std::map<std::string, ImageProviderInfo>::iterator iter;
287 for (iter = imageProviderInfoMap.begin(); iter != imageProviderInfoMap.end(); iter++)
289 outputText <<
" " << iter->first;
295 return waitForImages(imageProviderInfoMap.begin()->first, milliseconds);
298 bool ImageProcessor::waitForImages(std::string providerName,
int milliseconds)
300 std::shared_ptr<std::condition_variable> cond;
303 std::unique_lock lock(imageProviderInfoMutex);
305 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.find(providerName);
307 if (iter == imageProviderInfoMap.end())
309 ARMARX_ERROR <<
"Trying to wait for images from unknown image provider. Call useImageProvider before";
312 cond = iter->second.imageAvailableEvent;
316 ARMARX_DEBUG <<
"Waiting for images from provider proxy " << providerName;
320 std::unique_lock lock(mut);
321 auto td = std::chrono::milliseconds(milliseconds);
323 return cond->wait_for(lock, td) != std::cv_status::timeout;
326 bool ImageProcessor::waitForImages(std::string providerName,
IceUtil::Time waitTime)
328 return waitForImages(providerName,
static_cast<int>(waitTime.toMilliSeconds()));
331 bool ImageProcessor::isNewImageAvailable()
333 return isNewImageAvailable(imageProviderInfoMap.begin()->first);
336 bool ImageProcessor::isNewImageAvailable(
const std::string& providerName)
338 std::unique_lock lock(imageProviderInfoMutex);
339 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.find(providerName);
341 if (iter == imageProviderInfoMap.end())
343 ARMARX_ERROR <<
"Trying access unknown image provider. Call useImageProvider before";
347 bool result = iter->second.imageAvailable;
351 int ImageProcessor::getImages(CByteImage** ppImages)
353 int numberOfImages = getNumberOfImages();
359 else for (
int i = 0; i < numberOfImages; ++i)
367 void** ppBuffer =
new void* [numberOfImages];
369 for (
int i = 0; i < numberOfImages; ++i)
371 ppBuffer[i] = (
void*) ppImages[i]->pixels;
374 numberOfImages = getImages(ppBuffer);
377 return numberOfImages;
380 int ImageProcessor::getImages(CFloatImage** ppImages)
382 int numberOfImages = getNumberOfImages();
384 void** ppBuffer =
new void* [numberOfImages];
386 for (
int i = 0; i < numberOfImages; ++i)
388 ppBuffer[i] = (
void*) ppImages[i]->pixels;
391 numberOfImages = getImages(ppBuffer);
394 return numberOfImages;
397 int ImageProcessor::getImages(
void** ppBuffer)
399 if (imageProviderInfoMap.size() == 0)
401 ARMARX_ERROR <<
"Calling getImages without ImageProvider name, and no ImageProvider is available - did you forget to call useImageProvider?";
404 else if (imageProviderInfoMap.size() > 1)
406 std::string outputText =
"Several image providers are available - using the first one in the list:";
407 std::map<std::string, ImageProviderInfo>::iterator iter;
409 for (iter = imageProviderInfoMap.begin(); iter != imageProviderInfoMap.end(); iter++)
411 outputText <<
" " << iter->first;
416 std::string providerName;
418 std::unique_lock lock(imageProviderInfoMutex);
419 providerName = imageProviderInfoMap.begin()->first;
423 armarx::MetaInfoSizeBasePtr info;
424 return getImages(providerName, ppBuffer, info);
427 int ImageProcessor::getImages(std::string providerName, CByteImage** ppImages, armarx::MetaInfoSizeBasePtr& info)
429 int numberOfImages = getNumberOfImages(providerName);
431 void** ppBuffer =
new void* [numberOfImages];
433 for (
int i = 0; i < numberOfImages; ++i)
435 ppBuffer[i] = (
void*) ppImages[i]->pixels;
438 numberOfImages = getImages(providerName, ppBuffer, info);
441 return numberOfImages;
444 int ImageProcessor::getImages(std::string providerName,
const std::vector<CByteImageUPtr >& ppImages, MetaInfoSizeBasePtr& info)
446 int numberOfImages = getNumberOfImages(providerName);
448 void** ppBuffer =
new void* [numberOfImages];
450 for (
int i = 0; i < numberOfImages; ++i)
452 ppBuffer[i] = (
void*) ppImages.at(i)->pixels;
455 numberOfImages = getImages(providerName, ppBuffer, info);
458 return numberOfImages;
461 int ImageProcessor::getImages(std::string providerName, CFloatImage** ppImages, armarx::MetaInfoSizeBasePtr& info)
463 int numberOfImages = getNumberOfImages(providerName);
465 void** ppBuffer =
new void* [numberOfImages];
467 for (
int i = 0; i < numberOfImages; ++i)
469 ppBuffer[i] = (
void*) ppImages[i]->pixels;
472 numberOfImages = getImages(providerName, ppBuffer, info);
475 return numberOfImages;
478 int ImageProcessor::getImages(std::string providerName,
void** ppBuffer, armarx::MetaInfoSizeBasePtr& info)
480 int numberImages = -1;
484 std::unique_lock lock(imageProviderInfoMutex);
487 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.find(providerName);
489 if (iter == imageProviderInfoMap.end())
491 ARMARX_ERROR <<
"Trying to retrieve images from unknown image provider. Call useImageProvider before";
495 numberImages = iter->second.numberImages;
498 if (!iter->second.imageAvailable)
508 CompressedImageProviderInterfacePrx compressedImageProvider;
509 if (compressionType != eNoCompression)
511 compressedImageProvider = CompressedImageProviderInterfacePrx::checkedCast(iter->second.proxy);
512 if (!compressedImageProvider)
514 ARMARX_WARNING <<
deactivateSpam(10000000, iter->first) <<
"Trying to use image provider as CompressedImageProviderInterfacePrx, but it is not of this type: " << iter->second.proxy->ice_id();
517 if (compressionType != eNoCompression && compressedImageProvider)
520 auto blob = compressedImageProvider->getCompressedImagesAndMetaInfo(compressionType, compressionQuality, providerInfo.
info);
523 cv::Mat mat = cv::imdecode(blob, cv::IMREAD_COLOR);
525 ARMARX_DEBUG <<
deactivateSpam(10) <<
"decoded image to size " << mat.size().width <<
"x" << mat.size().height <<
", bpp: " << mat.channels();
526 providerInfo.
buffer = Blob(mat.datastart, mat.dataend);
534 blob = iter->second.proxy->getImagesAndMetaInfo(providerInfo.
info);
542 blob = iter->second.proxy->getImages();
545 blob.swap(providerInfo.
buffer);
550 usedImageProviders[providerName]->getData(providerInfo.
buffer, providerInfo.
info);
556 for (
int i = 0 ; i < numberImages ; i++)
561 info = iter->second.info;
564 iter->second.imageAvailable =
false;
570 std::unique_lock lock(statisticsMutex);
571 statistics[providerName].pollingFPS.update();
577 int ImageProcessor::getNumberOfImages(
const std::string& providerName)
579 if (imageProviderInfoMap.size() == 0 && providerName.empty())
581 ARMARX_ERROR <<
"Calling getNumberOfImages() without ImageProvider name, and no ImageProvider is available - did you forget to call useImageProvider?";
584 else if (imageProviderInfoMap.size() > 1 && providerName.empty())
586 std::string outputText =
"Several image providers are available and no name was specified when calling getNumberOfImages() - using the first one in the list:";
587 std::map<std::string, ImageProviderInfo>::iterator iter;
589 for (iter = imageProviderInfoMap.begin(); iter != imageProviderInfoMap.end(); iter++)
591 outputText <<
" " << iter->first;
597 int numberOfImages = 0;
599 if (providerName.empty())
601 std::unique_lock lock(imageProviderInfoMutex);
603 numberOfImages = imageProviderInfoMap.begin()->second.numberImages;
607 std::unique_lock lock(imageProviderInfoMutex);
610 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.find(providerName);
612 if (iter == imageProviderInfoMap.end())
614 ARMARX_ERROR <<
"Trying to retrieve images from unknown image provider. Call useImageProvider before";
618 numberOfImages = iter->second.numberImages;
621 return numberOfImages;
627 std::unique_lock lock(statisticsMutex);
629 std::map<std::string, ImageTransferStats>::iterator iter = statistics.find(providerName);
631 if (iter == statistics.end())
633 ARMARX_ERROR <<
"Requesting statistics for unknown image provider (" << providerName <<
")";
642 iter->second.pollingFPS.reset();
646 iter->second.imageProviderFPS.recalculate();
647 iter->second.pollingFPS.recalculate();
653 MetaInfoSizeBasePtr ImageProcessor::getImageMetaInfo(
const std::string& imageProviderName)
const
658 if (usedImageProviders.empty())
663 if (imageProviderName.empty())
665 return usedImageProviders.begin()->second->getMetaInfo();
669 auto it = usedImageProviders.find(imageProviderName);
670 if (it != usedImageProviders.end())
672 return it->second->getMetaInfo();
676 ARMARX_ERROR <<
"Requesting meta info for unknown image provider (" << imageProviderName <<
")";
682 void ImageProcessor::setFramerate(
float fps)
684 this->desiredFps = fps;
687 processorTask->changeInterval(1000.f / fps);
691 float ImageProcessor::getFramerate()
const
696 void ImageProcessor::setCompressionType(CompressionType compressionType,
int compressionQuality)
698 ARMARX_VERBOSE <<
"Setting compression to " << (int)compressionType <<
" and quality " << compressionQuality;
699 this->compressionType = compressionType;
700 this->compressionQuality = compressionQuality;
703 void ImageProcessor::reportImageAvailable(
const std::string& providerName,
const Ice::Current&
c)
707 std::unique_lock lock(imageProviderInfoMutex);
709 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.find(providerName);
711 if (iter == imageProviderInfoMap.end())
713 ARMARX_ERROR <<
deactivateSpam(10, providerName) <<
"Received notification from unknown imageprovider (" << providerName <<
")";
718 iter->second.imageAvailable =
true;
719 iter->second.imageAvailableEvent->notify_all();
720 ARMARX_DEBUG <<
"Got notification of image provider " << providerName;
724 std::unique_lock lock(statisticsMutex);
725 statistics[providerName].imageProviderFPS.update();
728 void ImageProcessor::cleanup()
730 std::map<std::string, ImageProviderInfo>::iterator iter = imageProviderInfoMap.begin();
732 while (iter != imageProviderInfoMap.end())
737 if (resultImageProvider)
739 getArmarXManager()->removeObjectBlocking(resultImageProvider);
743 void ImageProcessor::componentPropertiesUpdated(
const std::set<std::string>& changedProperties)
746 if (changedProperties.count(
"CompressionType"))
748 setCompressionType(getProperty<CompressionType>(
"CompressionType").getValue());
750 if (changedProperties.count(
"CompressionQuality"))
752 setCompressionType(getProperty<CompressionType>(
"CompressionType").getValue(), getProperty<int>(
"CompressionQuality").getValue());
758 int ImageProcessor::getCompressionQuality()
const
760 return compressionQuality;
763 CompressionType ImageProcessor::getCompressionType()
const
765 return compressionType;