diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt index f86bfcaea30cb170e21e2c0687a7fd25b5ae0314..a751e89b2566ed34fadcfa9c04943088955c1d5e 100644 --- a/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt +++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt @@ -12,7 +12,11 @@ armarx_component_set_name("ObjectMemoryGroup") set(COMPONENT_LIBS - ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers) + ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers + # RobotAPI + RobotAPIArmarXObjects + armem_objects +) set(SOURCES ObjectMemoryGroupRemoteStateOfferer.cpp diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml index 90e06380aac081c202bee6f38e0c8d8dca3421e4..48baf4ecf018d470ff57d6bfc11ed9011882dbb4 100644 --- a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml +++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml @@ -3,7 +3,9 @@ <Proxies> <Proxy value="RobotAPIInterfaces.MemoryNameSystem"/> </Proxies> - <Configurations/> - <State filename="RequestObjects.xml"/> + <Folder basename="test"> + <State filename="RequestObjectsTest.xml"/> + </Folder> + <State filename="RequestObjects.xml" visibility="public"/> </StatechartGroup> diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp index c2d0a05c91ce1760d5ca4d8f5ce44012296690fe..2142038a7b9b501ceb397ac914e91e6fc52f4cbb 100644 --- a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp +++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp @@ -28,32 +28,82 @@ namespace armarx::ObjectMemoryGroup void RequestObjects::run() { - std::vector<std::string> objectIdsString; + using Reader = armarx::armem::obj::instance::Reader; + + const std::string provider = in.isProviderSet() ? in.getProvider() : ""; + + const std::vector<std::string> objectIdsString = in.getObjectIds(); + const armarx::Duration relativeTimeout = armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds()); + + std::stringstream info; + std::stringstream warn; std::vector<armarx::ObjectID> objectIds; for (const std::string& idString : objectIdsString) { - + try + { + armarx::ObjectID id = armarx::ObjectID::FromString(idString); + objectIds.push_back(id); + info << "Requesting object " << id << "\n"; + } + catch (const armarx::LocalException& e) + { + warn << "\nGiven object ID '" << idString << "' could not parsed as ObjectID: " << e.what(); + } } - - using Reader = armarx::armem::obj::instance::Reader; - // put your user code for the execution-phase here - // runs in seperate thread, thus can do complex operations - // should check constantly whether isRunningTaskStopped() returns true - auto context = getContext<ObjectMemoryGroupStatechartContext>(); armarx::armem::client::MemoryNameSystem mns(getMemoryNameSystem(), context); + Reader reader{mns}; reader.connect(); - objpose::ObjectPoseStorageInterfacePrx storage = reader.getObjectPoseStorage(); armarx::objpose::observer::RequestObjectsInput input; - input.provider = ""; - input.request.objectIDs.clear(); + input.provider = provider; + for (const armarx::ObjectID& id : objectIds) + { + armarx::toIce(input.request.objectIDs.emplace_back(), id); + } + input.request.relativeTimeoutMS = relativeTimeout.toMilliSeconds(); + + + objpose::observer::RequestObjectsOutput output; + try + { + output = storage->requestObjects(input); + } + catch (const Ice::Exception& e) + { + ARMARX_WARNING << "Failed to request object localization: " << e.what(); + emitFailure(); + return; + } + + for (const auto& [id, result] : output.results) + { + if (result.result.success) + { + info << "Requested object " << id << " via provider '" << result.providerName << "'.\n"; + } + else + { + warn << "\nFailed to request the object " << id << " for localization."; + } + } + + if (not info.str().empty()) + { + ARMARX_INFO << info.str(); + } + if (not warn.str().empty()) + { + ARMARX_WARNING << "The following issues occurred whhen requesting objects for localization:" + << warn.str(); + } - storage->requestObjects(input); + emitSuccess(); } //void RequestObjects::onBreak() diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml index b3c13fed955c21a9e632ec57a012d6d2d3807e27..12a5c8bf4360f3c743dfa411a1aee51a396426b9 100644 --- a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml +++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml @@ -1,21 +1,19 @@ <?xml version="1.0" encoding="utf-8"?> <State version="1.2" name="RequestObjects" uuid="9F09FB2F-CC0C-4B8A-A716-F130E04A7230" width="800" height="600" type="Normal State"> - <InputParameters/> + <InputParameters> + <Parameter name="ObjectIds" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no"/> + <Parameter name="Provider" type="::armarx::StringVariantData" docType="string" optional="yes"/> + <Parameter name="RelativeTimeoutMilliseconds" type="::armarx::IntVariantData" docType="int" optional="no"/> + </InputParameters> <OutputParameters/> <LocalParameters/> - <Substates> - <EndState name="Failure" event="Failure" left="212.8" top="177.1" boundingSquareSize="100"/> - <EndState name="Success" event="Success" left="213.78" top="424.2" boundingSquareSize="100"/> - </Substates> + <Substates/> <Events> <Event name="Failure"> <Description>Event for statechart-internal failures or optionally user-code failures</Description> </Event> + <Event name="Success"/> </Events> - <StartState substateName="Success"> - <ParameterMappings/> - <SupportPoints/> - </StartState> <Transitions/> </State>