diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 8417e9fd8686b5813fe69f39f71c3ffecf328e44..6db9b9ad21f4f6cddeb48998b3aa4a82068c0d4e 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -258,37 +258,78 @@ namespace armarx } - objpose::RequestObjectsOutput ObjectPoseObserver::requestObjects( - const objpose::RequestObjectsInput& input, const Ice::Current&) + objpose::observer::RequestObjectsOutput ObjectPoseObserver::requestObjects( + const objpose::observer::RequestObjectsInput& input, const Ice::Current&) { - std::map<std::string, objpose::ObjectIDSeq> requests; + std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests; + std::map<std::string, objpose::ObjectPoseProviderPrx> proxies; + objpose::observer::RequestObjectsOutput output; + + auto updateProxy = [&](const std::string & providerName) + { + if (proxies.count(providerName) == 0) + { + if (auto it = providers.find(providerName); it != providers.end()) + { + proxies[providerName] = it->second.proxy; + } + else + { + ARMARX_ERROR << "No proxy for provider ' " << providerName << "'."; + proxies[providerName] = nullptr; + } + } + }; + + if (input.provider.size() > 0) + { + providerRequests[input.provider] = input.request; + updateProxy(input.provider); + } + else { std::scoped_lock lock(dataMutex); - for (const auto& objectID : input.objectIDs) + for (const auto& objectID : input.request.objectIDs) { - for (const auto& [name, info] : providers) + bool found = true; + for (const auto& [providerName, info] : providers) { // ToDo: optimize look up. if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end()) { - requests[name].push_back(objectID); + providerRequests[providerName].objectIDs.push_back(objectID); + updateProxy(providerName); + break; } } + if (!found) + { + ARMARX_ERROR << "Did not find a provider for " << objectID << "."; + output.results[objectID].providerName = ""; + } } } - for (const auto& [providerName, objects] : requests) + for (const auto& [providerName, request] : providerRequests) { - objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName); - if (provider) + if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy) { - ARMARX_VERBOSE << "Requesting provider '" << providerName << "' for objects " << objects; - provider->requestObjects(objects, input.relativeTimeoutMS); + ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects from provider '" + << providerName << "' for " << request.relativeTimeoutMS << " ms."; + objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request); + + int successful = 0; + for (const auto& [objectID, result] : providerOutput.results) + { + objpose::observer::ObjectRequestResult& res = output.results[objectID]; + res.providerName = providerName; + res.result = result; + successful += int(result.success); + } + ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful."; } } - objpose::RequestObjectsOutput output; - // todo return output; } diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h index 940c70d96ed6c6a43d14a7044df180fd854ecf5d..687db8ec03dfc84ab83a9e57da6e443d59e23dc5 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h @@ -91,7 +91,7 @@ namespace armarx objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override; objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override; - objpose::RequestObjectsOutput requestObjects(const objpose::RequestObjectsInput& input, ICE_CURRENT_ARG) override; + objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override; Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override; objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override; diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice index 3f311bdf13ce2d632679a1ceece4394f995267e5..aa3941e462fa8f09992d2e109ca209eb61f16538 100644 --- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice +++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice @@ -36,40 +36,43 @@ module armarx module objpose { - struct RequestObjectsInput + module observer { - ObjectIDSeq objectIDs; - long relativeTimeoutMS; - /** - * If there are multiple providers for the given IDs, prefer this one. - * If the specified provider is not available, take any. - */ - string preferredProvider; + struct RequestObjectsInput + { + /// The provider to request. If empty, any suitable provider per object ID is chosen. + string provider; + provider::RequestObjectsInput request; + }; + struct ObjectRequestResult + { + /// Name of the provider who was requested. + /// If empty, no suitable provider was found. + string providerName; + provider::ObjectRequestResult result; + }; + dictionary<ObjectID, ObjectRequestResult> ObjectRequestResultMap; + struct RequestObjectsOutput + { + /// The results per objectID. + ObjectRequestResultMap results; + }; }; - struct ObjectRequestResult - { - /// Name of the provider who was requested. - string providerName; - }; - dictionary<ObjectID, ObjectRequestResult> ObjectRequestResultMap; - struct RequestObjectsOutput - { - ObjectRequestResultMap results; - }; - interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic { + // Reading data::ObjectPoseSeq getObjectPoses(); data::ObjectPoseSeq getObjectPosesByProvider(string providerName); - RequestObjectsOutput requestObjects(RequestObjectsInput input); - - + bool hasProvider(string providerName); Ice::StringSeq getAvailableProviderNames(); ProviderInfoMap getAvailableProvidersInfo(); ProviderInfo getProviderInfo(string providerName); - bool hasProvider(string providerName); + + + // Modifying + observer::RequestObjectsOutput requestObjects(observer::RequestObjectsInput input); }; };