Add a FSM in MANN to handle the status of the advanceable

ami-iit · Apr 26, 2023 · 31b8e91 · 31b8e91
1 parent a1897a3
commit 31b8e91
Showing 1 changed file with 18 additions and 1 deletion.
diff --git a/src/ML/src/MANN.cpp b/src/ML/src/MANN.cpp
@@ -23,6 +23,11 @@ using namespace BipedalLocomotion;
 
 struct MANN::Impl
 {
+    enum class FSM
+    {
+        NotInitialized, Initialized, Running,
+    };
+
     struct DataStructured
     {
         BipedalLocomotion::System::VariablesHandler handler;
@@ -44,6 +49,8 @@ struct MANN::Impl
 
     Impl();
     bool populateInput(const MANNInput& input);
+
+    FSM state{FSM::NotInitialized};
 };
 
 MANN::Impl::Impl()
@@ -259,11 +266,18 @@ bool MANN::initialize(
     m_pimpl->output.jointVelocities.resize(numberOfJoints);
     m_pimpl->output.projectedBaseVelocity = manif::SE2Tangentd::Zero();
 
+    m_pimpl->state = Impl::FSM::Initialized;
+
     return true;
 }
 
 bool MANN::setInput(const MANNInput& input)
 {
+    if (m_pimpl->state != Impl::FSM::Initialized && m_pimpl->state != Impl::FSM::Running)
+    {
+        log()->error("[MANN::setInput] The network is not initialized, please call initialize()");
+        return false;
+    }
     return m_pimpl->populateInput(input);
 }
 
@@ -306,12 +320,15 @@ bool MANN::advance()
     unpackMatrix("base_velocity", tempVector);
     m_pimpl->output.projectedBaseVelocity
         = manif::SE2Tangentd(tempVector(0), tempVector(1), tempVector(2));
+
+    m_pimpl->state = Impl::FSM::Running;
+
     return true;
 }
 
 bool MANN::isOutputValid() const
 {
-    return true;
+    return m_pimpl->state == Impl::FSM::Running;
 }
 
 const MANNOutput& MANN::getOutput() const