Update to interfaces

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

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -13,8 +13,37 @@
 
 namespace AppleKraken
 {
-    void ApplePickerComponent::StartAutomatedOperation() // Probably to be connected to another bus
+    namespace Internal
     {
+        AZStd::string TaskString(const PickAppleTask& task)
+        {
+            if (task.m_appleEntityId.IsValid())
+            {
+                return "|Task for an unspecified apple|";
+            }
+
+            return AZStd::string::format("|Task for entity id %s|", task.m_appleEntityId.ToString().c_str());
+        }
+
+        AZStd::string CurrentTaskString(const AZStd::queue<PickAppleTask>& taskQueue)
+        {
+            if (taskQueue.empty())
+            {
+                return "|No task, pick queue empty!|";
+            }
+
+            const auto& currentTask = taskQueue.front();
+            return TaskString(currentTask);
+        }
+    } // namespace Internal
+
+    void ApplePickerComponent::StartAutomatedOperation()
+    {
+    }
+
+    float ApplePickerComponent::ReportProgress()
+    {
+        return 0.0f;
     }
 
     void ApplePickerComponent::Activate()
@@ -40,18 +69,20 @@ namespace AppleKraken
         }
     }
 
-    void ApplePickerComponent::ApplePicked(AZ::EntityId appleId)
+    void ApplePickerComponent::ApplePicked()
     {
-        AZ_TracePrintf("ApplePicker", "Picked apple id %s\n", appleId.ToString().c_str());
+        AZ_TracePrintf("ApplePicker", "%s. Picked apple\n", Internal::CurrentTaskString(m_currentAppleTasks).c_str());
     }
 
     void ApplePickerComponent::AppleRetrieved()
     {
-        AZ_TracePrintf("ApplePicker", "An apple has been retrieved and stored\n");
+        AZ_TracePrintf(
+            "ApplePicker", "%s. An apple has been retrieved and stored\n", Internal::CurrentTaskString(m_currentAppleTasks).c_str());
     }
 
-    void ApplePickerComponent::PickingFailed(AZ::EntityId appleId, const AZStd::string& reason)
+    void ApplePickerComponent::PickingFailed(const AZStd::string& reason)
     {
-        AZ_TracePrintf("ApplePicker", "Picking apple %s failed due to: %s\n", appleId.ToString().c_str(), reason.c_str());
+        AZ_TracePrintf(
+            "ApplePicker", "%s. Picking failed due to: %s\n", Internal::CurrentTaskString(m_currentAppleTasks).c_str(), reason.c_str());
     }
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickerComponent.h

@@ -17,16 +17,11 @@ namespace AppleKraken
     //! The component also acts as a ground-truth detector
     // using DetectionsMessage = vision_msgs::msgs::Detection3DArray;
 
-    struct AppleTask
-    {
-        AZ::EntityId m_appleEntityId;
-        AZ::Aabb m_appleBoundingBox;
-    };
-
     //! Demo component handling orchestration of apple picking
     class ApplePickerComponent
         : public AZ::Component
         , private ApplePickingNotificationBus::Handler // Probably could use TickBus as well for timeouts
+
     {
     public:
         AZ_COMPONENT(ApplePickerComponent, "{E9E83A4A-31A4-4E7A-AF88-7565AC8B9F27}", AZ::Component);
@@ -35,14 +30,20 @@ namespace AppleKraken
         void Deactivate() override;
         static void Reflect(AZ::ReflectContext* context);
 
+        //! Detect and pick all apples in manipulator range.
         void StartAutomatedOperation();
 
+        //! Report overall progress of gathering task.
+        //! @returns how much of the task is complete (0: nothing, 1: all of it). The task is completed when all reachable apples are
+        //! gathered (or had a failure) and the effector is in IDLE state.
+        float ReportProgress();
+
     private:
-        void ApplePicked(AZ::EntityId appleId) override;
+        void ApplePicked() override;
         void AppleRetrieved() override;
-        void PickingFailed(AZ::EntityId appleId, const AZStd::string& reason) override;
+        void PickingFailed(const AZStd::string& reason) override;
 
         AZ::Obb m_gatheringArea;
-        AZStd::queue<AppleTask> m_currentAppleTasks;
+        AZStd::queue<PickAppleTask> m_currentAppleTasks; //! Populated in StartAutomatedOperation. Tasks are popped when completed or failed.
     };
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickingNotifications.h

@@ -7,18 +7,25 @@
  */
 #pragma once
 
+#include "PickingStructs.h"
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/RTTI/BehaviorContext.h>
 
 namespace AppleKraken
 {
-    //! Notifications related to apple picking
+    //! Notifications related to apple picking.
     class ApplePickingNotifications : public AZ::EBusTraits
     {
     public:
-        virtual void ApplePicked(AZ::EntityId appleId) = 0;
+        //! An apple was successfully picked.
+        virtual void ApplePicked() = 0;
+
+        //! An apple was successfully retrieved to storage and can count as harvested.
         virtual void AppleRetrieved() = 0;
-        virtual void PickingFailed(AZ::EntityId appleId, const AZStd::string& reason) = 0;
+
+        //! Apple picking failed
+        //! @param reason reason for failure, e.g. "out of reach", "apple not found", "approach obstructed", etc.
+        virtual void PickingFailed(const AZStd::string& reason) = 0;
     };
 
     using ApplePickingNotificationBus = AZ::EBus<ApplePickingNotifications>;

Project/Gem/Source/ApplePicker/ApplePickingRequests.h

@@ -7,6 +7,7 @@
  */
 #pragma once
 
+#include "PickingStructs.h"
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Interface/Interface.h>
 #include <AzCore/Math/Aabb.h>
@@ -25,18 +26,24 @@ namespace AppleKraken
         virtual void PrepareForPicking() = 0;
 
         //! PickApple by its global bounding box.
-        //! @param globalAppleBoundingBox global bounding box for the apple.
-        //! @param appleId
-        //! Note this could be changed to local Aabb for real use cases.
-        //! Global can be queried once, local is less resilient to robot instability, mini movement.
-        //! Note that this can get a while and result will be signalled through ApplePickingNotifications.
-        virtual void PickApple(AZ::Aabb globalAppleBoundingBox, AZ::EntityId appleId) = 0;
+        //! @param appleTask task structure specifying which apple to pick.
+        //! This task should only be issued if effector state is PREPARED.
+        //! Note that the task can get a while and result will be signalled through ApplePickingNotifications.
+        //! Progress can be checked @see GetEffectorState.
+        //! This function returns immediately.
+        virtual void PickApple(const PickAppleTask& appleTask) = 0;
 
         //! Request to store currently held apple and retrieve manipulator into a travel position.
+        //! This function returns immediately. The effector should respond by transitioning to an IDLE state.
         virtual void FinishPicking() = 0;
 
+        //! Request current effector state.
+        //! This request should be called to inform about whether a task can be issued and whether the robot could start moving.
+        //! @returns current state of the effector.
+        virtual PickingState GetEffectorState() = 0;
+
         //! Return area covered by effector.
-        //! @returns an object bounding box which is a region where apples can be picked.
+        //! @returns a global object bounding box which is a region where apples can be picked.
         virtual AZ::Obb GetEffectorReachArea() = 0;
     };
 

Project/Gem/Source/ApplePicker/PickingStructs.h

@@ -0,0 +1,39 @@
+/*
+ * 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 <AzCore/Component/EntityId.h>
+#include <AzCore/Math/Aabb.h>
+
+namespace AppleKraken
+{
+    //! Important states of the Kraken effector (manipulator with a vacuum nozzle)
+    enum EffectorState
+    {
+        IDLE, //!< Idle state / position, suitable for robot moving around the environment.
+        PREPARED, //!< State and position which are ready for picking tasks.
+        PICKING, //!< The effector is on its way to pick fruit.
+        RETRIEVING, //!< The effector is retrieving a fruit to storage position.
+        INVALID //!< Invalid state. Requires an additional context that could help user understand what happened. @see PickingState.
+    };
+
+    //! A structure holding a state of effector, including optional progress and descriptive information.
+    struct PickingState
+    {
+        EffectorState m_effectorState = IDLE; //!< Current state of effector.
+        float m_taskProgress = 0.0f; //!< Optional field signalling progress within current state (picking/retrieving).
+        AZStd::string m_description; //!< Optional descriptive field to inform the user.
+    };
+
+    //! A task to pick a single apple.
+    struct PickAppleTask
+    {
+        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.
+    };
+} // namespace AppleKraken

Project/Gem/roscondemo_files.cmake

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