From 45e10866c9be4fd46a7457ed95f11942b0b4595a Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 26 Jul 2023 14:01:35 +0200
Subject: [PATCH] (+CD) Implement RequestObjects state
---
.../ObjectMemoryGroup/CMakeLists.txt | 6 +-
.../ObjectMemoryGroup.scgxml | 6 +-
.../ObjectMemoryGroup/RequestObjects.cpp | 74 ++++++++++++++++---
.../ObjectMemoryGroup/RequestObjects.xml | 16 ++--
4 files changed, 78 insertions(+), 24 deletions(-)
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt
index f86bfcaea..a751e89b2 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 90e06380a..48baf4ecf 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 c2d0a05c9..2142038a7 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 b3c13fed9..12a5c8bf4 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>
--
GitLab