Add lambdas to the Kraken State Machine

Signed-off-by: alek-kam-robotec-ai <[email protected]>

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -22,19 +22,8 @@ namespace AppleKraken
     namespace DebugStateTransit
     {
         // TODO - this is a debug space for a stub implementation. Proper: a state transition machine with lambdas.
-        struct HashTransition
-        {
-            size_t operator()(const std::pair<EffectorState, EffectorState>& p) const
-            {
-                int16_t first = static_cast<int16_t>(p.first);
-                int16_t second = static_cast<int16_t>(p.second);
-                size_t combined = (size_t)first << 16 | second;
-                return combined;
-            }
-        };
-
         // <startState, targetState>, debugTime
-        using TransitionMap = AZStd::unordered_map<StateTransition, float, HashTransition>;
+        using TransitionMap = AZStd::unordered_map<StateTransition, float, TransitionHash>;
 
         TransitionMap GetTransitionMap()
         {
@@ -67,6 +56,11 @@ namespace AppleKraken
         }
     } // namespace DebugStateTransit
 
+    KrakenEffectorComponent::KrakenEffectorComponent()
+    {
+        InitializeTransitionActions();
+    }
+
     void KrakenEffectorComponent::Activate()
     {
         ApplePickingRequestBus::Handler::BusConnect(GetEntityId());
@@ -144,6 +138,51 @@ namespace AppleKraken
         ManipulatorRequestHandler::Reflect(context);
     }
 
+    void KrakenEffectorComponent::InitializeTransitionActions()
+    {
+        m_stateTransitionActions = {
+            {
+                { EffectorState::IDLE, EffectorState::PREPARED },
+                [this]()
+                {
+                    LockManipulator(false);
+                    ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::EffectorReadyForPicking);
+                },
+            },
+            {
+                { EffectorState::PREPARED, EffectorState::PICKING },
+                []()
+                {
+                },
+            },
+            {
+                { EffectorState::PICKING, EffectorState::RETRIEVING },
+                [this]()
+                {
+                    if (!m_currentTask.IsValid())
+                    {
+                        AZ_Error("KrakenEffectorComponent", true, "No valid task for current picking!");
+                        return;
+                    }
+                    ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::Retrieve);
+                },
+            },
+            {
+                { EffectorState::RETRIEVING, EffectorState::PREPARED },
+                []()
+                {
+                    ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::AppleRetrieved);
+                },
+            },
+            {
+                { EffectorState::PREPARED, EffectorState::IDLE },
+                []()
+                {
+                },
+            },
+        };
+    }
+
     bool KrakenEffectorComponent::IsTransitionValid(EffectorState targetState) const
     {
         auto transitionKey = std::make_pair(m_effectorState, targetState);
@@ -274,25 +313,16 @@ namespace AppleKraken
         return reachArea;
     }
 
-    void KrakenEffectorComponent::OnEffectorReadyForPicking()
+    std::function<void()> KrakenEffectorComponent::GetTransitionAction(EffectorState currentState, EffectorState targetState)
     {
-        LockManipulator(false);
-        ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::EffectorReadyForPicking);
-    }
-
-    void KrakenEffectorComponent::OnApplePicked()
-    {
-        if (!m_currentTask.IsValid())
+        auto transitActionIt = m_stateTransitionActions.find(StateTransition{ currentState, targetState });
+        if (transitActionIt == m_stateTransitionActions.end())
         {
-            AZ_Error("KrakenEffectorComponent", true, "No valid task for current picking!");
-            return;
+            AZ_Assert(false, "No function found for provided state");
+            return {};
         }
-        ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::Retrieve);
-    }
 
-    void KrakenEffectorComponent::OnAppleRetrieved()
-    {
-        ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::AppleRetrieved);
+        return transitActionIt->second;
     }
 
     void KrakenEffectorComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
@@ -332,20 +362,9 @@ namespace AppleKraken
         auto previousState = m_effectorState;
         m_effectorState = m_effectorTargetState;
 
-        if (previousState == EffectorState::IDLE && m_effectorState == EffectorState::PREPARED)
-        {
-            OnEffectorReadyForPicking();
-        }
+        auto onTransitionFinished = GetTransitionAction(previousState, m_effectorState);
+        onTransitionFinished();
 
-        if (previousState == EffectorState::PICKING && m_effectorState == EffectorState::RETRIEVING)
-        {
-            OnApplePicked();
-        }
-
-        if (previousState == EffectorState::RETRIEVING && m_effectorState == EffectorState::PREPARED)
-        {
-            OnAppleRetrieved();
-        }
         if (!m_registeredCallback)
         {
             auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -31,12 +31,13 @@ namespace AppleKraken
     {
     public:
         AZ_COMPONENT(KrakenEffectorComponent, "{9206FC30-DF56-4246-8247-5D6B31603B53}");
-        KrakenEffectorComponent() = default;
+        KrakenEffectorComponent();
         static void Reflect(AZ::ReflectContext* context);
 
     private:
         void Activate() override;
         void Deactivate() override;
+        void InitializeTransitionActions();
 
         void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
 
@@ -45,21 +46,20 @@ namespace AppleKraken
         void FinishPicking() override;
         PickingState GetEffectorState() override;
         AZ::Obb GetEffectorReachArea() override;
+        std::function<void()> GetTransitionAction(EffectorState currentState, EffectorState targetState);
 
         void BeginTransitionIfAcceptable(EffectorState targetState);
         bool IsTransitionAcceptable(EffectorState targetState) const;
         bool IsTransitionValid(EffectorState targetState) const;
 
-        void OnEffectorReadyForPicking();
-        void OnApplePicked();
-        void OnAppleRetrieved();
-
         void LockManipulator(bool locked);
 
         PickAppleTask m_currentTask; //!> valid if RETRIEVING or PICKING
         EffectorState m_effectorState = EffectorState::IDLE;
         EffectorState m_effectorTargetState = EffectorState::IDLE;
         float m_currentStateTransitionTime = 0.0f;
+        // <startState, targetState>, onTransitionFinished
+        AZStd::unordered_map<StateTransition, std::function<void()>, TransitionHash> m_stateTransitionActions;
         AZ::EntityId m_reachEntity;
         AZ::EntityId m_manipulatorEntity;
         AZ::EntityId m_rootEntityToFreeze;

Project/Gem/Source/ApplePicker/PickingStructs.h

@@ -50,4 +50,15 @@ namespace AppleKraken
     };
 
     using StateTransition = std::pair<EffectorState, EffectorState>;
+
+    struct TransitionHash
+    {
+        size_t operator()(const std::pair<EffectorState, EffectorState>& p) const
+        {
+            int16_t first = static_cast<int16_t>(p.first);
+            int16_t second = static_cast<int16_t>(p.second);
+            size_t combined = (size_t)first << 16 | second;
+            return combined;
+        }
+    };
 } // namespace AppleKraken