Apple picker interface

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

.clang-format

@@ -0,0 +1,61 @@
+Language: Cpp
+
+AccessModifierOffset: -4
+AlignAfterOpenBracket: AlwaysBreak
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Right
+AlignOperands: false
+AlignTrailingComments: false
+AllowAllArgumentsOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortFunctionsOnASingleLine: None
+AllowShortLambdasOnASingleLine: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: false
+BinPackParameters: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+    AfterClass: true
+    AfterControlStatement: true
+    AfterEnum: true
+    AfterFunction: true
+    AfterNamespace: true
+    BeforeLambdaBody: true
+    AfterStruct: true
+    BeforeElse: true
+    SplitEmptyFunction: true
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeComma
+BreakInheritanceList: BeforeComma
+ColumnLimit: 140
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: false
+FixNamespaceComments: true
+IncludeBlocks: Preserve
+IndentCaseBlocks: true
+IndentCaseLabels: false
+IndentPPDirectives: None
+IndentWidth: 4
+KeepEmptyLinesAtTheStartOfBlocks: false
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: All
+PenaltyReturnTypeOnItsOwnLine: 1000
+PointerAlignment: Left
+SortIncludes: true
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyParentheses: false
+SpacesInAngles: false
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+Standard: c++17
+UseTab: Never

Project/Gem/CMakeLists.txt

@@ -40,6 +40,8 @@ ly_add_target(
             Gem::Atom_AtomBridge.Static
 )
 
+#target_depends_on_ros2_packages(ROSConDemo.Static vision_msgs)
+
 ly_add_target(
     NAME ROSConDemo ${PAL_TRAIT_MONOLITHIC_DRIVEN_MODULE_TYPE}
     NAMESPACE Gem

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -0,0 +1,57 @@
+/*
+ * 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 "ApplePickerComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+// #include <ROS2/ROS2Bus.h>
+
+namespace AppleKraken
+{
+    void ApplePickerComponent::StartAutomatedOperation() // Probably to be connected to another bus
+    {
+    }
+
+    void ApplePickerComponent::Activate()
+    {
+    }
+
+    void ApplePickerComponent::Deactivate()
+    {
+    }
+
+    void ApplePickerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ApplePickerComponent, AZ::Component>()->Version(1);
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<ApplePickerComponent>("Apple picking component", "A demo component for apple picking")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "AppleKraken");
+            }
+        }
+    }
+
+    void ApplePickerComponent::ApplePicked(AZ::EntityId appleId)
+    {
+        AZ_TracePrintf("ApplePicker", "Picked apple id %s\n", appleId.ToString().c_str());
+    }
+
+    void ApplePickerComponent::AppleRetrieved()
+    {
+        AZ_TracePrintf("ApplePicker", "An apple has been retrieved and stored\n");
+    }
+
+    void ApplePickerComponent::PickingFailed(AZ::EntityId appleId, const AZStd::string& reason)
+    {
+        AZ_TracePrintf("ApplePicker", "Picking apple %s failed due to: %s\n", appleId.ToString().c_str(), reason.c_str());
+    }
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickerComponent.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 "ApplePickingNotifications.h"
+#include "ApplePickingRequests.h"
+#include <AzCore/Component/Component.h>
+// #include <vision_msgs/msgs/detection_3d_array.h>
+
+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);
+        ApplePickerComponent() = default;
+        void Activate() override;
+        void Deactivate() override;
+        static void Reflect(AZ::ReflectContext* context);
+
+        void StartAutomatedOperation();
+
+    private:
+        void ApplePicked(AZ::EntityId appleId) override;
+        void AppleRetrieved() override;
+        void PickingFailed(AZ::EntityId appleId, const AZStd::string& reason) override;
+
+        AZ::Obb m_gatheringArea;
+        AZStd::queue<AppleTask> m_currentAppleTasks;
+    };
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickingNotifications.h

@@ -0,0 +1,25 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/RTTI/BehaviorContext.h>
+
+namespace AppleKraken
+{
+    //! Notifications related to apple picking
+    class ApplePickingNotifications : public AZ::EBusTraits
+    {
+    public:
+        virtual void ApplePicked(AZ::EntityId appleId) = 0;
+        virtual void AppleRetrieved() = 0;
+        virtual void PickingFailed(AZ::EntityId appleId, const AZStd::string& reason) = 0;
+    };
+
+    using ApplePickingNotificationBus = AZ::EBus<ApplePickingNotifications>;
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickingRequests.h

@@ -0,0 +1,51 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Interface/Interface.h>
+#include <AzCore/Math/Aabb.h>
+#include <AzCore/Math/Obb.h>
+
+namespace AppleKraken
+{
+    //! Requests for apple picking effector (manipulator)
+    class ApplePickingRequests
+    {
+    public:
+        AZ_RTTI(ApplePickingRequests, "{E70BC163-4AE0-4660-9769-1C3C7C3493A6}");
+        virtual ~ApplePickingRequests() = default;
+
+        //! Request to prepare for incoming apple picking tasks. Could be empty if manipulator is always ready.
+        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;
+
+        //! Request to store currently held apple and retrieve manipulator into a travel position.
+        virtual void FinishPicking() = 0;
+
+        //! Return area covered by effector.
+        //! @returns an object bounding box which is a region where apples can be picked.
+        virtual AZ::Obb GetEffectorReachArea() = 0;
+    };
+
+    class ApplePickingBusTraits : public AZ::EBusTraits
+    {
+    public:
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single;
+    };
+    using ApplePickingRequestBus = AZ::EBus<ApplePickingRequests, ApplePickingBusTraits>;
+    using ApplePickingInterface = AZ::Interface<ApplePickingRequests>;
+} // namespace AppleKraken

Project/Gem/roscondemo_files.cmake

@@ -1,7 +1,11 @@
 
 set(FILES
-    Include/ROSConDemo/ROSConDemoBus.h
-    Source/ROSConDemoSystemComponent.cpp
-    Source/ROSConDemoSystemComponent.h
-    enabled_gems.cmake
-)
+        Include/ROSConDemo/ROSConDemoBus.h
+        Source/ApplePicker/ApplePickerComponent.cpp
+        Source/ApplePicker/ApplePickerComponent.h
+        Source/ApplePicker/ApplePickingNotifications.h
+        Source/ApplePicker/ApplePickingRequests.h
+        Source/ROSConDemoSystemComponent.cpp
+        Source/ROSConDemoSystemComponent.h
+        enabled_gems.cmake
+        )

README.md

@@ -7,6 +7,16 @@ For information about contributing to Open 3D Engine, visit [https://o3de.org/do
 
 ## Download and Install
 
+### Additional ROS 2 packages
+
+This project uses Vision messages, which can be obtained:
+
+#### For ROS 2 Humble:
+`sudo apt install ros-humble-vision-msgs`
+
+#### For ROS 2 Galactic
+`sudo apt install ros-galactic-vision-msgs`
+
 ### Clone the repository 
 
 ```shell