Skip to content
Snippets Groups Projects
Commit 45e10866 authored by Rainer Kartmann's avatar Rainer Kartmann Committed by ARMAR-6
Browse files

(+CD) Implement RequestObjects state

parent 050914f7
No related branches found
No related tags found
1 merge request!370Request object statechart
......@@ -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
......
......@@ -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>
......@@ -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()
......
<?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>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment