(+CD) Implement RequestObjects state

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

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>
 

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()

source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml

 <?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>