sorting by chute distance

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

Project/Assets/Importer/apple_kraken_5.prefab

@@ -2425,7 +2425,8 @@
                     "m_template": {
                         "$type": "ApplePickerComponent",
                         "EffectorEntity": "Entity_[126373983206044]",
-                        "FruitStorageEntity": "Entity_[126373983206044]"
+                        "FruitStorageEntity": "Entity_[126373983206044]",
+                        "RetrievalPointEntity": "Entity_[453828565735265]"
                     }
                 },
                 "Component_[15792714784584127302]": {
@@ -2563,7 +2564,8 @@
                         "Entity_[50844409631117]",
                         "Entity_[9058513484485]",
                         "Entity_[5494828217205]",
-                        "Entity_[36172097579699]"
+                        "Entity_[36172097579699]",
+                        "Entity_[453828565735265]"
                     ]
                 },
                 "Component_[3226220641517151136]": {
@@ -3432,7 +3434,7 @@
                     "Id": 12122484710959107959,
                     "Controller": {
                         "Configuration": {
-                            "EditorEntityId": 11583927400651280762
+                            "EditorEntityId": 7939196949259269833
                         }
                     }
                 },
@@ -3498,6 +3500,56 @@
                 }
             }
         },
+        "Entity_[453828565735265]": {
+            "Id": "Entity_[453828565735265]",
+            "Name": "RetrievalChute",
+            "Components": {
+                "Component_[12171884485533334948]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 12171884485533334948
+                },
+                "Component_[12574217689782564474]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 12574217689782564474
+                },
+                "Component_[12999122222194170561]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 12999122222194170561
+                },
+                "Component_[13504040141164984746]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 13504040141164984746
+                },
+                "Component_[13555347149641495242]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 13555347149641495242,
+                    "Parent Entity": "Entity_[126373983206044]",
+                    "Transform Data": {
+                        "Translate": [
+                            1.333984375,
+                            -0.23783111572265625,
+                            1.3847572803497314
+                        ]
+                    }
+                },
+                "Component_[15247268977907304446]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 15247268977907304446
+                },
+                "Component_[16426304834367737653]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 16426304834367737653
+                },
+                "Component_[2692724083108161793]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 2692724083108161793
+                },
+                "Component_[6950308048593391230]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 6950308048593391230
+                }
+            }
+        },
         "Entity_[49624638919053]": {
             "Id": "Entity_[49624638919053]",
             "Name": "Effector",

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -187,11 +187,12 @@ namespace AppleKraken
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serialize->Class<ApplePickerComponent, AZ::Component>()
-                ->Version(3)
+                ->Version(4)
                 ->Field("TriggerServiceTopic", &ApplePickerComponent::m_triggerServiceTopic)
                 ->Field("CancelServiceTopic", &ApplePickerComponent::m_cancelServiceTopic)
                 ->Field("EffectorEntity", &ApplePickerComponent::m_effectorEntityId)
-                ->Field("FruitStorageEntity", &ApplePickerComponent::m_fruitStorageEntityId);
+                ->Field("FruitStorageEntity", &ApplePickerComponent::m_fruitStorageEntityId)
+                ->Field("RetrievalPointEntity", &ApplePickerComponent::m_retrievalPointEntityId);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -218,7 +219,12 @@ namespace AppleKraken
                         AZ::Edit::UIHandlers::EntityId,
                         &ApplePickerComponent::m_fruitStorageEntityId,
                         "Fruit Storage",
-                        "Fruit storage entity");
+                        "Fruit storage entity")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &ApplePickerComponent::m_retrievalPointEntityId,
+                        "Fruit retrieval point",
+                        "Entity which holds the point of the retrieval chute");
             }
         }
     }
@@ -296,7 +302,11 @@ namespace AppleKraken
 
     void ApplePickerComponent::QueryEnvironmentForAllApplesInBox(const AZ::Obb& globalBox)
     {
-        // TODO - query environment
+        if (!m_retrievalPointEntityId.IsValid())
+        {
+            AZ_Error("ApplePicker", false, "Retrieval chute entity not set for ApplePickerComponent!");
+            return;
+        }
 
         // Scene query for `apple` entity, we want visible entities with exact 'Apple' name
         auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
@@ -349,13 +359,16 @@ namespace AppleKraken
                 found_apples.emplace(r.m_entityId);
             }
         }
+        AZ::Transform m_retrievalPointTransform;
+        AZ::TransformBus::EventResult(m_retrievalPointTransform, m_retrievalPointEntityId, &AZ::TransformBus::Events::GetWorldTM);
+        auto retrievalPoint = m_retrievalPointTransform.GetTranslation();
 
         std::sort(
             appleTasks.begin(),
             appleTasks.end(),
-            [globalBox](const PickAppleTask& a, const PickAppleTask& b) -> bool
-            { // a is further than b from the globalBox
-                return globalBox.GetDistance(a.m_middle) > globalBox.GetDistance(b.m_middle);
+            [retrievalPoint](const PickAppleTask& a, const PickAppleTask& b) -> bool
+            { // a is closer than b to the retrieval point
+                return (retrievalPoint - a.m_middle).GetLengthSq() <= (retrievalPoint - b.m_middle).GetLengthSq();
             });
 
         m_appleGroundTruthDetector->UpdateGroundTruth(appleTasks);

Project/Gem/Source/ApplePicker/ApplePickerComponent.h

@@ -62,6 +62,9 @@ namespace AppleKraken
         AZ::EntityId m_effectorEntityId;
         AZ::EntityId m_fruitStorageEntityId;
 
+        // TODO - actually use this entity for retrieval position
+        AZ::EntityId m_retrievalPointEntityId; //!< used to sort apples by distance to retrieval chute
+
         rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_triggerService;
         rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_cancelService;
         size_t m_initialTasksSize = 0;