Move statechart memory to skills memory

source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt

@@ -2,7 +2,7 @@ armarx_component_set_name("SkillsMemory")
 
 
 set(COMPONENT_LIBS
-    ArmarXCore ArmarXCoreInterfaces  # for DebugObserverInterface
+    ArmarXCore ArmarXCoreInterfaces ArmarXCoreObservers # for DebugObserverInterface
     ArmarXGuiComponentPlugins
     RobotAPICore RobotAPIInterfaces armem
     # RobotAPIComponentPlugins  # for ArViz and other plugins
@@ -12,13 +12,22 @@ set(COMPONENT_LIBS
 
 set(SOURCES
     SkillsMemory.cpp
+    StatechartListener.cpp
 )
 set(HEADERS
     SkillsMemory.h
+    StatechartListener.h
 )
 
 armarx_add_component("${SOURCES}" "${HEADERS}")
 
+armarx_enable_aron_file_generation_for_target(
+    TARGET_NAME
+        ${ARMARX_COMPONENT_NAME}
+    ARON_FILES
+        aron/StatechartData.xml
+)
+
 #generate the application
 armarx_generate_and_add_component_executable()
 

source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp

@@ -22,9 +22,12 @@
 
 #include "SkillsMemory.h"
 
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
 
-#include <SimoxUtility/algorithm/string.h>
+//#include <SimoxUtility/algorithm/string.h>
 
 #include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
@@ -38,6 +41,18 @@ namespace armarx
     armarx::PropertyDefinitionsPtr SkillsMemory::createPropertyDefinitions()
     {
         armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        // Publish
+        defs->topic(debugObserver);
+
+        // Subscribe
+        //        defs->defineOptionalProperty("tpc.sub.ProfilerListener", p.statechartTransitionsTopicName,
+        //                                     "Name of the ProfilerListenerInterface topics to subscribe.");
+        defs->optional(p.statechartTransitionsTopicName, "tpc.sub.ProfilerListener", "Name of the ProfilerListenerInterface topics to subscribe.");
+
+        const std::string prefix = "mem.";
+        defs->optional(p.memoryName, prefix + "MemoryName", "Name of this memory server.");
+
         return defs;
     }
 
@@ -50,17 +65,30 @@ namespace armarx
 
     void SkillsMemory::onInitComponent()
     {
-        workingMemory.name() = memoryName;
+        workingMemory.name() = p.memoryName;
+
+        //        getProperty(p.statechartTransitionsTopicName, "tpc.sub.StatechartListener");
+
+        {
+            armem::data::CoreSegment& core = workingMemory.addCoreSegment("Statechart");
+            workingMemory.
+            //            statechartTransitionsProviderSegmentID = core.p  ("Transitions",armarx::statechart::aron::Transition::toInitialAronType()).id();
+        }
     }
 
 
     void SkillsMemory::onConnectComponent()
     {
+        statechartListener = createStatechartListener(p.statechartTransitionsTopicName);
     }
 
 
     void SkillsMemory::onDisconnectComponent()
     {
+        if (statechartListener)
+        {
+            getArmarXManager()->removeObjectBlocking(statechartListener->getName());
+        }
     }
 
 
@@ -73,7 +101,7 @@ namespace armarx
     // WRITING
     armem::data::AddSegmentsResult SkillsMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
     {
-        armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
+        armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments({input}, p.core.addOnUsage);
         return result;
     }
 
@@ -88,4 +116,83 @@ namespace armarx
     // READING
     // Inherited from Plugin
 
+
+    IceInternal::Handle<StatechartListener>
+    SkillsMemory::createStatechartListener(const std::string& topicName, const std::string& _name)
+    {
+        const std::string name = _name.empty() ? topicName + ".Listener" : _name;
+        ARMARX_DEBUG << "Registering StatechartListener '" << name << "' listening to topic '" << topicName << "'.";
+
+        IceInternal::Handle<armarx::StatechartListener> listener = Component::create<armarx::StatechartListener>();
+        listener->setName(name);
+        listener->setTopicName(topicName);
+
+        // Callback for the transition listener
+        listener->registerCallback(
+            [this](const std::vector<StatechartListener::Transition>& transitions,
+                   armarx::StatechartListener & source)
+        {
+            this->reportTransitions(transitions);
+        });
+
+        try
+        {
+            getArmarXManager()->addObject(listener);
+        }
+        catch (const Ice::AlreadyRegisteredException& e)
+        {
+            ARMARX_ERROR << "The name '" << name << "' is already used. Please choose another one.\n"
+                         << "Reason: " << e.what();
+            getArmarXManager()->removeObjectBlocking(name);
+        }
+        listener->getObjectScheduler()->waitForObjectState(armarx::ManagedIceObjectState::eManagedIceObjectStarted);
+
+        return listener;
+    }
+
+    void
+    SkillsMemory::reportTransitions(const std::vector<StatechartListener::Transition>& transitions)
+    {
+        for (const StatechartListener::Transition& t : transitions)
+        {
+            const std::string& entityName = std::to_string(t.processId);
+            IceUtil::Time transitionTime = IceUtil::Time::microSeconds(t.timestamp);
+
+            armem::EntityUpdate update;
+            update.entityID = statechartTransitionsProviderSegmentID.withEntityName(entityName);
+            update.timeCreated = transitionTime;
+
+
+            armarx::statechart::aron::Transition data;
+            data.processId = t.processId;
+            data.sourceStateIdentifier = t.sourceStateIdentifier;
+            data.targetStateIdentifier = t.targetStateIdentifier;
+            data.eventName = t.eventName;
+            data.targetStateType = toAronStateEnum[t.targetStateType];
+            data.inputParameters = *toAronParameterMap(t.inputParameters);
+            data.localParameters = *toAronParameterMap(t.localParameters);
+            data.outputParameters = *toAronParameterMap(t.outputParameters);
+
+            update.instancesData.push_back(data.toAron());
+
+            try
+            {
+                workingMemory.update(update);
+            }
+            catch (const armem::error::ArMemError& e)
+            {
+                ARMARX_WARNING << e.what();
+            }
+        }
+    }
+
+    std::shared_ptr<armarx::statechart::aron::ParameterMap> SkillsMemory::toParameterMap(const armarx::StateParameterMap& map)
+    {
+        std::shared_ptr<armarx::statechart::aron::ParameterMap> aronMap(new armarx::statechart::aron::ParameterMap);
+        for (auto const& [key, val] : map)
+        {
+            aronMap->parameters[key] = val->value->toString();
+        }
+        return aronMap;
+    }
 }

source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h

@@ -31,6 +31,9 @@
 
 #include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
 
+#include <RobotAPI/components/armem/server/SkillsMemory/StatechartListener.h>
+#include <RobotAPI/components/armem/server/RobotActionMemory/aron/StatechartData.aron.generated.h>
+
 
 namespace armarx
 {
@@ -80,7 +83,40 @@ namespace armarx
 
 
     private:
-        std::string memoryName = "SkillsMemory";
-        bool addCoreSegmentOnUsage = true;
+        DebugObserverInterfacePrx debugObserver;
+
+        struct Properties
+        {
+            std::string memoryName = "Skills";
+
+            std::string statechartTransitionsTopicName = "StateReportingTopic";
+
+            struct CoreSegments
+            {
+                //                std::vector<std::string> defaultCoreSegments = { "Transitions" };
+                bool addOnUsage = false;
+            };
+            CoreSegments core;
+        };
+        Properties p;
+
+        // Statechart transition logging
+        armem::MemoryID statechartTransitionsProviderSegmentID;
+
+        IceInternal::Handle<StatechartListener> createStatechartListener(const std::string& topicName,
+                const std::string& _name = "");
+
+        IceInternal::Handle<armarx::StatechartListener> statechartListener;
+        void reportTransitions(const std::vector<StatechartListener::Transition>& transitions);
+
+        std::map<armarx::eStateType, armarx::statechart::aron::StateType> toAronStateEnum =
+        {
+            {eNormalState, armarx::statechart::aron::StateType::NORMAL},
+            {eFinalState, armarx::statechart::aron::StateType::FINAL},
+            {eRemoteState, armarx::statechart::aron::StateType::REMOTE},
+            {eDynamicRemoteState, armarx::statechart::aron::StateType::DYNAMIC_REMOTE},
+            {eUndefined, armarx::statechart::aron::StateType::UNDEFINED},
+        };
+        static std::shared_ptr<armarx::statechart::aron::ParameterMap> toAronParameterMap(const armarx::StateParameterMap&);
     };
 }

source/RobotAPI/components/armem/server/SkillsMemory/StatechartListener.cpp

+#include "StatechartListener.h"
+
+
+namespace armarx
+{
+    void StatechartListener::setName(const std::string& name)
+    {
+        armarx::Component::setName(name);
+    }
+
+    void StatechartListener::setTopicName(const std::string& name)
+    {
+        this->topicName = name;
+    }
+
+    std::string StatechartListener::getTopicName() const
+    {
+        return topicName;
+    }
+
+    StatechartListener::~StatechartListener() = default;
+
+    std::string StatechartListener::getDefaultName() const
+    {
+        return "StatechartListener";
+    }
+
+    void StatechartListener::onInitComponent()
+    {
+        ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+        usingTopic(topicName);
+    }
+    void StatechartListener::onConnectComponent()
+    {
+        ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+    }
+
+    void StatechartListener::registerCallback(const StatechartListener::Callback& callback)
+    {
+        callbacks.push_back(callback);
+    }
+
+    void StatechartListener::publish(const std::vector<Transition>& message)
+    {
+        for (Callback& callback : callbacks)
+        {
+            callback(message, *this);
+        }
+    }
+
+    void
+    StatechartListener::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& transition,
+            const Ice::Current&)
+    {
+        publish({transition});
+    }
+
+    void StatechartListener::reportStatechartTransitionWithParametersList(
+        const ProfilerStatechartTransitionWithParametersList& transitions, const Ice::Current&)
+    {
+        publish(transitions);
+    }
+}

source/RobotAPI/components/armem/server/SkillsMemory/StatechartListener.h

+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+
+namespace armarx
+{
+    class StatechartListener :
+        virtual public armarx::Component
+        , virtual public armarx::ProfilerListener
+    {
+    public:
+        using Transition = armarx::ProfilerStatechartTransitionWithParameters;
+        using Callback = std::function<void(const std::vector<StatechartListener::Transition>& transitions, StatechartListener& source)>;
+
+    public:
+        ~StatechartListener() override;
+
+        void setTopicName(const std::string& topicName);
+        std::string getTopicName() const;
+
+        void setName(const std::string& name);
+        void registerCallback(const Callback& callback);
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+    protected:
+        void onInitComponent() override;
+        void onConnectComponent() override;
+
+        // ProfilerListener interface
+    public:
+        void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
+        void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+
+        void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
+        void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
+        void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
+        void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+        void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+        void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
+        void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
+        void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
+
+        void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
+        void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
+        void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
+        void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+        void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+        void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
+        void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+
+
+    private:
+        std::string topicName;
+
+        std::vector<Callback> callbacks;
+        void publish(const std::vector<Transition>& message);
+    };
+}

source/RobotAPI/components/armem/server/SkillsMemory/aron/StatechartData.xml

+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+    </CodeIncludes>
+
+    <GenerateTypes>
+        <IntEnum name="armarx::statechart::aron::StateType">
+            <EnumValue key="NORMAL" value="0" />
+            <EnumValue key="FINAL" value="1" />
+            <EnumValue key="REMOTE" value="2" />
+            <EnumValue key="DYNAMIC_REMOTE" value="3" />
+            <EnumValue key="UNDEFINED" value="4" />
+        </IntEnum>
+
+        <Object name='armarx::statechart::aron::ParameterMap'>
+            <ObjectChild key='parameters'>
+                <dict>
+                    <String />
+                </dict>
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::statechart::aron::Transition'>
+            <ObjectChild key='processId'>
+                <int />
+            </ObjectChild>
+
+            <ObjectChild key="sourceStateIdentifier">
+                <String />
+            </ObjectChild>
+
+            <ObjectChild key="targetStateIdentifier">
+                <String />
+            </ObjectChild>
+
+            <ObjectChild key="eventName">
+                <String />
+            </ObjectChild>
+
+            <ObjectChild key="targetStateType">
+                <armarx::statechart::aron::StateType />
+            </ObjectChild>
+
+            <ObjectChild key="inputParameters">
+                <armarx::statechart::aron::ParameterMap />
+            </ObjectChild>
+
+            <ObjectChild key="localParameters">
+                <armarx::statechart::aron::ParameterMap />
+            </ObjectChild>
+
+            <ObjectChild key="outputParameters">
+                <armarx::statechart::aron::ParameterMap />
+            </ObjectChild>
+        </Object>
+    </GenerateTypes>
+</AronTypeDefinition>