Initial implementation (with some stubs) of orchestration

Signed-off-by: Adam Dabrowski <[email protected]>

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -7,6 +7,8 @@
  */
 
 #include "ApplePickerComponent.h"
+#include "ApplePickingRequests.h"
+#include <AzCore/EBus/Event.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 
@@ -38,19 +40,58 @@ namespace AppleKraken
 
     void ApplePickerComponent::StartAutomatedOperation()
     {
+        if (!m_currentAppleTasks.empty())
+        {
+            AZ_Error("ApplePicker", false, "Tasks still in progress for current picking!");
+            return;
+        }
+
+        // Get effector reach
+        AZ::Obb effectorRangeGlobalBox;
+        ApplePickingRequestBus::EventResult(
+            effectorRangeGlobalBox, m_effectorEntityId, &ApplePickingRequests::GetEffectorReachArea);
+
+        // Find out apples within the reach
+        QueryEnvironmentForAllApplesInBox(effectorRangeGlobalBox);
+
+        // Tell effector to prepare for picking
+        ApplePickingRequestBus::Event(m_effectorEntityId, &ApplePickingRequests::PrepareForPicking);
+    }
+
+    void ApplePickerComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        // TODO handle timeouts and incoming commands
+
+        // TODO - a debug loop
+        if (m_currentAppleTasks.empty())
+        {
+            StartAutomatedOperation();
+        }
     }
 
     float ApplePickerComponent::ReportProgress()
     {
-        return 0.0f;
+        // TODO (minor) - take into consideration current task progress (effector state)
+        if (m_initialTasksSize == 0)
+        {
+            AZ_Warning("ApplePicker", false, "ReportProgress reporting 1 since no apples were found in the call");
+            return 1.0f;
+        }
+
+        return 1.0f - (m_currentAppleTasks.size() / m_initialTasksSize);
     }
 
     void ApplePickerComponent::Activate()
     {
+        m_effectorEntityId = GetEntityId(); // TODO - remove this once we expose this field
+        ApplePickingNotificationBus::Handler::BusConnect();
+        AZ::TickBus::Handler::BusConnect();
     }
 
     void ApplePickerComponent::Deactivate()
     {
+        AZ::TickBus::Handler::BusDisconnect();
+        ApplePickingNotificationBus::Handler::BusDisconnect();
     }
 
     void ApplePickerComponent::Reflect(AZ::ReflectContext* context)
@@ -68,20 +109,66 @@ namespace AppleKraken
         }
     }
 
+    void ApplePickerComponent::EffectorReadyForPicking()
+    {
+        PickNextApple();
+    }
+
     void ApplePickerComponent::ApplePicked()
     {
+        if (m_currentAppleTasks.empty())
+        {
+            AZ_Error("ApplePicker", false, "ApplePicked called but no current task");
+            return;
+        }
         AZ_TracePrintf("ApplePicker", "%s. Picked apple\n", Internal::CurrentTaskString(m_currentAppleTasks).c_str());
     }
 
     void ApplePickerComponent::AppleRetrieved()
     {
+        if (m_currentAppleTasks.empty())
+        {
+            AZ_Error("ApplePicker", false, "AppleRetrieved called but no current task");
+            return;
+        }
         AZ_TracePrintf(
             "ApplePicker", "%s. An apple has been retrieved and stored\n", Internal::CurrentTaskString(m_currentAppleTasks).c_str());
+        m_currentAppleTasks.pop();
+        PickNextApple();
     }
 
     void ApplePickerComponent::PickingFailed(const AZStd::string& reason)
-    {
+    { // TODO - refactor common code (debugs, checks)
+        if (m_currentAppleTasks.empty())
+        {
+            AZ_Error("ApplePicker", false, "PickingFailed called but no current task");
+            return;
+        }
         AZ_TracePrintf(
             "ApplePicker", "%s. Picking failed due to: %s\n", Internal::CurrentTaskString(m_currentAppleTasks).c_str(), reason.c_str());
+        m_currentAppleTasks.pop();
+        PickNextApple();
     }
+
+    void ApplePickerComponent::PickNextApple()
+    {
+        if (!m_currentAppleTasks.empty())
+        { // Get another apple!
+            ApplePickingRequestBus::Event(m_effectorEntityId, &ApplePickingRequests::PickApple, m_currentAppleTasks.front());
+            return;
+        }
+    }
+
+    void ApplePickerComponent::QueryEnvironmentForAllApplesInBox(const AZ::Obb& /*globalBox*/)
+    {
+        // TODO - query environment
+
+        // Debug
+        for (int i = 0; i < 5; ++i)
+        {
+            PickAppleTask emptyTask;
+            m_currentAppleTasks.push(emptyTask);
+        }
+    }
+
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickerComponent.h

@@ -10,21 +10,19 @@
 #include "ApplePickingNotifications.h"
 #include "ApplePickingRequests.h"
 #include <AzCore/Component/Component.h>
-// #include <vision_msgs/msgs/detection_3d_array.h>
+#include <AzCore/Component/TickBus.h>
 
 namespace AppleKraken
 {
     //! Demo component handling orchestration of apple picking
     class ApplePickerComponent
         : public AZ::Component
-        , private ApplePickingNotificationBus::Handler // Probably could use TickBus as well for timeouts
-
+        , public ApplePickingNotificationBus::Handler
+        , public AZ::TickBus::Handler
     {
     public:
         AZ_COMPONENT(ApplePickerComponent, "{E9E83A4A-31A4-4E7A-AF88-7565AC8B9F27}", AZ::Component);
         ApplePickerComponent() = default;
-        void Activate() override;
-        void Deactivate() override;
         static void Reflect(AZ::ReflectContext* context);
 
         //! Detect and pick all apples in manipulator range.
@@ -36,11 +34,23 @@ namespace AppleKraken
         float ReportProgress();
 
     private:
+        void Activate() override;
+        void Deactivate() override;
+
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        void EffectorReadyForPicking() override;
         void ApplePicked() override;
         void AppleRetrieved() override;
         void PickingFailed(const AZStd::string& reason) override;
 
+        void PickNextApple();
+        void QueryEnvironmentForAllApplesInBox(const AZ::Obb& globalBox);
+
+        AZ::EntityId m_effectorEntityId;
         AZ::Obb m_gatheringArea;
-        AZStd::queue<PickAppleTask> m_currentAppleTasks; //! Populated in StartAutomatedOperation. Tasks are popped when completed or failed.
+        size_t m_initialTasksSize = 0;
+        AZStd::queue<PickAppleTask>
+            m_currentAppleTasks; //! Populated in StartAutomatedOperation. Tasks are popped when completed or failed.
     };
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickingNotifications.h

@@ -17,7 +17,10 @@ namespace AppleKraken
     class ApplePickingNotifications : public AZ::EBusTraits
     {
     public:
-        //! An apple was successfully picked.
+        //! The effector is ready for picking
+        virtual void EffectorReadyForPicking() = 0;
+
+         //! An apple was successfully picked.
         virtual void ApplePicked() = 0;
 
         //! An apple was successfully retrieved to storage and can count as harvested.

Project/Gem/Source/ApplePicker/ApplePickingRequests.h

@@ -8,18 +8,21 @@
 #pragma once
 
 #include "PickingStructs.h"
+#include <AzCore/Component/ComponentBus.h>
 #include <AzCore/EBus/EBus.h>
-#include <AzCore/Interface/Interface.h>
+#include <AzCore/EBus/Event.h>
 #include <AzCore/Math/Aabb.h>
 #include <AzCore/Math/Obb.h>
 
 namespace AppleKraken
 {
     //! Requests for apple picking effector (manipulator)
-    class ApplePickingRequests
+    class ApplePickingRequests : public AZ::EBusTraits
     {
     public:
-        AZ_RTTI(ApplePickingRequests, "{E70BC163-4AE0-4660-9769-1C3C7C3493A6}");
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+        using BusIdType = AZ::EntityId;
+
         virtual ~ApplePickingRequests() = default;
 
         //! Request to prepare for incoming apple picking tasks. Could be empty if manipulator is always ready.
@@ -47,12 +50,5 @@ namespace AppleKraken
         virtual AZ::Obb GetEffectorReachArea() = 0;
     };
 
-    class ApplePickingBusTraits : public AZ::EBusTraits
-    {
-    public:
-        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple;
-        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single;
-    };
-    using ApplePickingRequestBus = AZ::EBus<ApplePickingRequests, ApplePickingBusTraits>;
-    using ApplePickingInterface = AZ::Interface<ApplePickingRequests>;
+    using ApplePickingRequestBus = AZ::EBus<ApplePickingRequests>;
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "KrakenEffectorComponent.h"
+#include "ApplePickingNotifications.h"
+#include "PickingStructs.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+
+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>;
+
+        TransitionMap GetTransitionMap()
+        {
+            static const TransitionMap tm = {
+                { std::make_pair(EffectorState::IDLE, EffectorState::PREPARED), 0.1f },
+                { std::make_pair(EffectorState::PREPARED, EffectorState::PICKING), 0.5f },
+                { std::make_pair(EffectorState::PICKING, EffectorState::RETRIEVING), 2.0f },
+                { std::make_pair(EffectorState::RETRIEVING, EffectorState::PREPARED), 2.0f },
+                { std::make_pair(EffectorState::PREPARED, EffectorState::IDLE), 0.1f },
+            };
+            return tm;
+        }
+
+        AZStd::string StateTransitionString(EffectorState current, EffectorState next)
+        {
+            return AZStd::string::format("state transition %d -> %d\n", static_cast<int>(current), static_cast<int>(next));
+        }
+
+        float GetStateTransitTime(EffectorState currentState, EffectorState targetState)
+        {
+            auto transitions = GetTransitionMap();
+            auto key = std::make_pair(currentState, targetState);
+            if (transitions.contains(key))
+            {
+                return GetTransitionMap().at(key);
+            }
+
+            // Invalid state transitions detected elsewhere
+            return 0.0f;
+        }
+    } // namespace DebugStateTransit
+
+    void KrakenEffectorComponent::Activate()
+    {
+        ApplePickingRequestBus::Handler::BusConnect(GetEntityId());
+        AZ::TickBus::Handler::BusConnect();
+    }
+
+    void KrakenEffectorComponent::Deactivate()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        ApplePickingRequestBus::Handler::BusDisconnect();
+    }
+
+    void KrakenEffectorComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<KrakenEffectorComponent, AZ::Component>()->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<KrakenEffectorComponent>("Kraken Effector", "Manipulator component for picking apples")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"));
+            }
+        }
+    }
+
+    bool KrakenEffectorComponent::IsTransitionValid(EffectorState targetState) const
+    {
+        auto transitionKey = std::make_pair(m_effectorState, targetState);
+        if (DebugStateTransit::GetTransitionMap().contains(transitionKey))
+        {
+            return true;
+        }
+        return false;
+    }
+
+    bool KrakenEffectorComponent::IsTransitionAcceptable(EffectorState targetState) const
+    {
+        if (m_effectorState != m_effectorTargetState)
+        {
+            AZ_Error(
+                "KrakenEffectorComponent",
+                false,
+                "Unable to accept request: currently realizing %s",
+                DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
+            return false;
+        }
+
+        if (!IsTransitionValid(targetState))
+        {
+            AZ_Error(
+                "KrakenEffectorComponent",
+                false,
+                "Invalid state transition %s",
+                DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
+            return false;
+        }
+
+        return true;
+    }
+
+    void KrakenEffectorComponent::BeginTransitionIfAcceptable(EffectorState targetState)
+    {
+        if (IsTransitionAcceptable(targetState))
+        {
+            m_currentStateTransitionTime = 0.0f;
+            m_effectorTargetState = targetState;
+        }
+    }
+
+    void KrakenEffectorComponent::PrepareForPicking()
+    {
+        AZ_TracePrintf("KrakenEffectorComponent", "PrepareForPicking\n");
+        BeginTransitionIfAcceptable(EffectorState::PREPARED);
+    }
+
+    void KrakenEffectorComponent::PickApple(const PickAppleTask& appleTask)
+    {
+        AZ_TracePrintf("KrakenEffectorComponent", "PickApple\n");
+        // TODO - handle appleTask
+        BeginTransitionIfAcceptable(EffectorState::PICKING);
+    }
+
+    void KrakenEffectorComponent::FinishPicking()
+    {
+        AZ_TracePrintf("KrakenEffectorComponent", "FinishPicking\n");
+        BeginTransitionIfAcceptable(EffectorState::IDLE);
+    }
+
+    PickingState KrakenEffectorComponent::GetEffectorState()
+    {
+        PickingState state;
+        state.m_effectorState = m_effectorState;
+        state.m_taskProgress = 0.0f; // TODO
+        return state;
+    }
+
+    AZ::Obb KrakenEffectorComponent::GetEffectorReachArea()
+    {
+        AZ_TracePrintf("KrakenEffectorComponent", "GetEffectorReachArea\n");
+        AZ::Obb reachArea;
+        // TODO - get the actual area
+        return reachArea;
+    }
+
+    void KrakenEffectorComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (m_effectorState == m_effectorTargetState)
+        { // //TODO - nothing to do in stub version
+            return;
+        }
+
+        m_currentStateTransitionTime += deltaTime;
+        if (m_currentStateTransitionTime < DebugStateTransit::GetStateTransitTime(m_effectorState, m_effectorTargetState))
+        {
+            return;
+        }
+
+        // State transition
+        AZ_TracePrintf(
+            "KrakenEffectorComponent", "%s", DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
+        m_currentStateTransitionTime = 0.0f;
+        m_effectorState = m_effectorTargetState;
+
+        if (m_effectorState == EffectorState::PICKING && m_effectorTargetState == EffectorState::RETRIEVING)
+        {
+            // TODO - also handle picking failed
+            ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::ApplePicked);
+
+            // Start retrieving right away
+            m_effectorTargetState = EffectorState::RETRIEVING;
+            return;
+        }
+
+        if (m_effectorState == EffectorState::RETRIEVING && m_effectorTargetState == EffectorState::PREPARED)
+        {
+            ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::AppleRetrieved);
+            return;
+        }
+    }
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include "ApplePickingRequests.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzCore/EBus/EBus.h>
+
+namespace AppleKraken
+{
+    //! Component for apple picking effector (manipulator)
+    class KrakenEffectorComponent
+        : public AZ::Component
+        , public ApplePickingRequestBus::Handler
+        , public AZ::TickBus::Handler
+    {
+    public:
+        AZ_COMPONENT(KrakenEffectorComponent, "{9206FC30-DF56-4246-8247-5D6B31603B53}");
+        KrakenEffectorComponent() = default;
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        void Activate() override;
+        void Deactivate() override;
+
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        void PrepareForPicking() override;
+        void PickApple(const PickAppleTask& appleTask) override;
+        void FinishPicking() override;
+        PickingState GetEffectorState() override;
+        AZ::Obb GetEffectorReachArea() override;
+
+        void BeginTransitionIfAcceptable(EffectorState targetState);
+        bool IsTransitionAcceptable(EffectorState targetState) const;
+        bool IsTransitionValid(EffectorState targetState) const;
+
+        EffectorState m_effectorState = EffectorState::IDLE;
+        EffectorState m_effectorTargetState = EffectorState::IDLE;
+        float m_currentStateTransitionTime = 0.0f;
+    };
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/PickingStructs.h

@@ -13,13 +13,13 @@
 namespace AppleKraken
 {
     //! Important states of the Kraken effector (manipulator with a vacuum nozzle)
-    enum class EffectorState
+    enum class EffectorState : int16_t
     {
+        INVALID = -1, //!< Invalid state. Requires an additional context that could help user understand what happened. @see PickingState.
         IDLE = 0, //!< Idle state / position, suitable for robot moving around the environment.
         PREPARED = 10, //!< State and position which are ready for picking tasks.
         PICKING = 20, //!< The effector is on its way to pick fruit.
-        RETRIEVING = 30, //!< The effector is retrieving a fruit to storage position.
-        INVALID = -1 //!< Invalid state. Requires an additional context that could help user understand what happened. @see PickingState.
+        RETRIEVING = 30 //!< The effector is retrieving a fruit to storage position.
     };
 
     //! A structure holding a state of effector, including optional progress and descriptive information.
@@ -36,4 +36,6 @@ namespace AppleKraken
         AZ::EntityId m_appleEntityId; //!< EntityId of the apple. Can be Invalid if the information is not available (check IsValid()).
         AZ::Aabb m_appleBoundingBox; //!< Bounding box of the apple to pick.
     };
+
+    using StateTransition = std::pair<EffectorState, EffectorState>;
 } // namespace AppleKraken

Project/Gem/Source/ROSConDemoModule.cpp

@@ -1,13 +1,13 @@
 
+#include "ApplePicker/ApplePickerComponent.h"
+#include "ApplePicker/KrakenEffectorComponent.h"
+#include "ROSConDemoSystemComponent.h"
 #include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/Module/Module.h>
 
-#include "ROSConDemoSystemComponent.h"
-
 namespace ROSConDemo
 {
-    class ROSConDemoModule
-        : public AZ::Module
+    class ROSConDemoModule : public AZ::Module
     {
     public:
         AZ_RTTI(ROSConDemoModule, "{E38575E4-7D2F-4617-B938-416E8C1C07B4}", AZ::Module);
@@ -17,9 +17,13 @@ namespace ROSConDemo
             : AZ::Module()
         {
             // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here.
-            m_descriptors.insert(m_descriptors.end(), {
-                ROSConDemoSystemComponent::CreateDescriptor(),
-            });
+            m_descriptors.insert(
+                m_descriptors.end(),
+                {
+                    ROSConDemoSystemComponent::CreateDescriptor(),
+                    AppleKraken::ApplePickerComponent::CreateDescriptor(),
+                    AppleKraken::KrakenEffectorComponent::CreateDescriptor(),
+                });
         }
 
         /**
@@ -32,6 +36,6 @@ namespace ROSConDemo
             };
         }
     };
-}// namespace ROSConDemo
+} // namespace ROSConDemo
 
 AZ_DECLARE_MODULE_CLASS(Gem_ROSConDemo, ROSConDemo::ROSConDemoModule)

Project/Gem/Source/ROSConDemoSystemComponent.cpp

@@ -2,7 +2,6 @@
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
-
 #include "ROSConDemoSystemComponent.h"
 
 namespace ROSConDemo

Project/Gem/Source/ROSConDemoSystemComponent.h

@@ -2,7 +2,6 @@
 #pragma once
 
 #include <AzCore/Component/Component.h>
-
 #include <ROSConDemo/ROSConDemoBus.h>
 
 namespace ROSConDemo
@@ -25,16 +24,8 @@ namespace ROSConDemo
         ~ROSConDemoSystemComponent();
 
     protected:
-        ////////////////////////////////////////////////////////////////////////
-        // ROSConDemoRequestBus interface implementation
-
-        ////////////////////////////////////////////////////////////////////////
-
-        ////////////////////////////////////////////////////////////////////////
-        // AZ::Component interface implementation
         void Init() override;
         void Activate() override;
         void Deactivate() override;
-        ////////////////////////////////////////////////////////////////////////
     };
 }

Project/Gem/roscondemo_files.cmake

@@ -6,6 +6,8 @@ set(FILES
         Source/ApplePicker/ApplePickingNotifications.h
         Source/ApplePicker/ApplePickingRequests.h
         Source/ApplePicker/PickingStructs.h
+        Source/ApplePicker/KrakenEffectorComponent.cpp
+        Source/ApplePicker/KrakenEffectorComponent.h
         Source/ROSConDemoSystemComponent.cpp
         Source/ROSConDemoSystemComponent.h
         enabled_gems.cmake