Merge branch 'request-object-statechart' into 'master'

Request object statechart

See merge request !370

source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h

@@ -74,6 +74,11 @@ namespace armarx::armem::obj::instance
             return this->p;
         }
 
+        objpose::ObjectPoseStorageInterfacePrx getObjectPoseStorage() const
+        {
+            return objPoseStorage;
+        }
+
     private:
         Properties p;
 

source/RobotAPI/statecharts/CMakeLists.txt

@@ -13,3 +13,5 @@ add_subdirectory(StatechartExecutionGroup)
 
 add_subdirectory(ProsthesisKinestheticTeachIn)
 add_subdirectory(DebugDrawerToArVizGroup)
+
+add_subdirectory(ObjectMemoryGroup)

source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt

+# Modern CMake way:
+#armarx_add_statechart(ObjectMemoryGroup
+#    SOURCES
+#        ObjectMemoryGroupRemoteStateOfferer.cpp
+#    HEADERS
+#        ObjectMemoryGroupRemoteStateOfferer.h
+#    DEPENDENCIES
+#)
+
+# Legacy CMake way:
+
+armarx_component_set_name("ObjectMemoryGroup")
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+    # RobotAPI
+    RobotAPIArmarXObjects
+    armem_objects
+)
+
+set(SOURCES
+    ObjectMemoryGroupRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+    ObjectMemoryGroupRemoteStateOfferer.h
+    ObjectMemoryGroup.scgxml
+)
+
+armarx_generate_statechart_cmake_lists()
+armarx_add_component("${SOURCES}" "${HEADERS}")

source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml

+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="ObjectMemoryGroup" package="RobotAPI" generateContext="true">
+	<Proxies>
+		<Proxy value="RobotAPIInterfaces.MemoryNameSystem"/>
+	</Proxies>
+	<Folder basename="test">
+		<State filename="RequestObjectsTest.xml"/>
+	</Folder>
+	<State filename="RequestObjects.xml" visibility="public"/>
+</StatechartGroup>
+

source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp

+#include "ObjectMemoryGroupRemoteStateOfferer.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+    // DO NOT EDIT NEXT LINE
+    ObjectMemoryGroupRemoteStateOfferer::SubClassRegistry ObjectMemoryGroupRemoteStateOfferer::Registry(ObjectMemoryGroupRemoteStateOfferer::GetName(), &ObjectMemoryGroupRemoteStateOfferer::CreateInstance);
+
+    ObjectMemoryGroupRemoteStateOfferer::ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+        XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > (reader)
+    {}
+
+    void ObjectMemoryGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer() {}
+
+    void ObjectMemoryGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer() {}
+
+    void ObjectMemoryGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer() {}
+
+    // DO NOT EDIT NEXT FUNCTION
+    std::string ObjectMemoryGroupRemoteStateOfferer::GetName()
+    {
+        return "ObjectMemoryGroupRemoteStateOfferer";
+    }
+
+    // DO NOT EDIT NEXT FUNCTION
+    XMLStateOffererFactoryBasePtr ObjectMemoryGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+    {
+        return XMLStateOffererFactoryBasePtr(new ObjectMemoryGroupRemoteStateOfferer(reader));
+    }
+}

source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h

+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "ObjectMemoryGroupStatechartContext.generated.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+    class ObjectMemoryGroupRemoteStateOfferer :
+        virtual public XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+    {
+    public:
+        ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+        // inherited from RemoteStateOfferer
+        void onInitXMLRemoteStateOfferer() override;
+        void onConnectXMLRemoteStateOfferer() override;
+        void onExitXMLRemoteStateOfferer() override;
+
+        // static functions for AbstractFactory Method
+        static std::string GetName();
+        static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+        static SubClassRegistry Registry;
+
+
+    };
+}

source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp

+#include "RequestObjects.h"
+
+//#include <ArmarXCore/core/time/TimeUtil.h>
+//#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include "ObjectMemoryGroupStatechartContext.generated.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+    // DO NOT EDIT NEXT LINE
+    RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(), &RequestObjects::CreateInstance);
+
+    RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData):
+        XMLStateTemplate<RequestObjects>(stateData), RequestObjectsGeneratedBase<RequestObjects>(stateData)
+    {
+    }
+
+    void RequestObjects::onEnter()
+    {
+
+        // put your user code for the enter-point here
+        // execution time should be short (<100ms)
+    }
+
+    void RequestObjects::run()
+    {
+        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();
+            }
+        }
+
+        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 = 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();
+        }
+
+        emitSuccess();
+    }
+
+    //void RequestObjects::onBreak()
+    //{
+    //    // put your user code for the breaking point here
+    //    // execution time should be short (<100ms)
+    //}
+
+    void RequestObjects::onExit()
+    {
+        // put your user code for the exit point here
+        // execution time should be short (<100ms)
+    }
+
+
+    // DO NOT EDIT NEXT FUNCTION
+    XMLStateFactoryBasePtr RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
+    {
+        return XMLStateFactoryBasePtr(new RequestObjects(stateData));
+    }
+}

source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h

+#pragma once
+
+#include <RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.generated.h>
+
+namespace armarx::ObjectMemoryGroup
+{
+    class RequestObjects :
+        public RequestObjectsGeneratedBase < RequestObjects >
+    {
+    public:
+        RequestObjects(const XMLStateConstructorParams& stateData);
+
+        // inherited from StateBase
+        void onEnter() override;
+        void run() override;
+        // void onBreak() override;
+        void onExit() override;
+
+        // static functions for AbstractFactory Method
+        static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+        static SubClassRegistry Registry;
+
+        // DO NOT INSERT ANY CLASS MEMBERS,
+        // use stateparameters instead,
+        // if classmember are neccessary nonetheless, reset them in onEnter
+    };
+}

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>
+		<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/>
+	<Events>
+		<Event name="Failure">
+			<Description>Event for statechart-internal failures or optionally user-code failures</Description>
+		</Event>
+		<Event name="Success"/>
+	</Events>
+	<Transitions/>
+</State>
+

source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml

+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="RequestObjectsTest" uuid="E74E1D8D-2E97-4846-9282-73AC4F49FAEE" width="493.167" height="355.278" type="Normal State">
+	<InputParameters>
+		<Parameter name="ObjectIds" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+			<DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/spraybottle"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/handwash-paste"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/mounting-adhesive"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/multipurpose-grease"}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="Maintenance/spraybottle\nMaintenance/handwash-paste\nMaintenance/mounting-adhesive\nMaintenance/multipurpose-grease"/>
+		</Parameter>
+		<Parameter name="Provider" type="::armarx::StringVariantData" docType="string" optional="no">
+			<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"sim_track"}}' docValue="sim_track"/>
+		</Parameter>
+		<Parameter name="RelativeTimeoutMilliseconds" type="::armarx::IntVariantData" docType="int" optional="no">
+			<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::IntVariantData","value":10000}}' docValue="10000"/>
+		</Parameter>
+	</InputParameters>
+	<OutputParameters/>
+	<LocalParameters/>
+	<Substates>
+		<EndState name="Failure" event="Failure" left="363.333" top="140.056" boundingSquareSize="99.6636"/>
+		<LocalState name="RequestObjects" refuuid="9F09FB2F-CC0C-4B8A-A716-F130E04A7230" left="114.167" top="196.278" boundingSquareSize="99.6636"/>
+		<EndState name="Success" event="Success" left="363.333" top="250.53" boundingSquareSize="99.6636"/>
+	</Substates>
+	<Events>
+		<Event name="Failure">
+			<Description>Event for statechart-internal failures or optionally user-code failures</Description>
+		</Event>
+	</Events>
+	<StartState substateName="RequestObjects">
+		<ParameterMappings>
+			<ParameterMapping sourceType="Parent" from="ObjectIds" to="ObjectIds"/>
+			<ParameterMapping sourceType="Parent" from="Provider" to="Provider"/>
+			<ParameterMapping sourceType="Parent" from="RelativeTimeoutMilliseconds" to="RelativeTimeoutMilliseconds"/>
+		</ParameterMappings>
+		<SupportPoints>
+			<SupportPoint posX="67.1067" posY="233.278"/>
+			<SupportPoint posX="76.5839" posY="233.278"/>
+			<SupportPoint posX="88.7241" posY="233.278"/>
+			<SupportPoint posX="101.186" posY="233.278"/>
+		</SupportPoints>
+	</StartState>
+	<Transitions>
+		<Transition from="RequestObjects" to="Failure" eventName="Failure">
+			<ParameterMappings/>
+			<ParameterMappingsToParentsLocal/>
+			<ParameterMappingsToParentsOutput/>
+			<SupportPoints>
+				<SupportPoint posX="214.588" posY="221.9"/>
+				<SupportPoint posX="253.892" posY="213.031"/>
+				<SupportPoint posX="308.683" posY="200.669"/>
+				<SupportPoint posX="350.786" posY="191.175"/>
+			</SupportPoints>
+		</Transition>
+		<Transition from="RequestObjects" to="Success" eventName="Success">
+			<ParameterMappings/>
+			<ParameterMappingsToParentsLocal/>
+			<ParameterMappingsToParentsOutput/>
+			<SupportPoints>
+				<SupportPoint posX="214.358" posY="252.153"/>
+				<SupportPoint posX="221.884" posY="254.613"/>
+				<SupportPoint posX="229.602" posY="256.935"/>
+				<SupportPoint posX="237" posY="258.833"/>
+				<SupportPoint posX="274.158" posY="268.372"/>
+				<SupportPoint posX="316.529" posY="275.551"/>
+				<SupportPoint posX="350.428" posY="280.424"/>
+			</SupportPoints>
+		</Transition>
+	</Transitions>
+</State>
+