Gathering rows api and implementation. Added to scene

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

Project/Gem/CMakeLists.txt

@@ -49,7 +49,7 @@ ly_add_target(
             Gem::Atom_AtomBridge.Static
 )
 
-target_depends_on_ros2_packages(ROSConDemo.Static std_srvs vision_msgs)
+target_depends_on_ros2_packages(ROSConDemo.Static std_srvs nav_msgs vision_msgs)
 
 ly_add_target(
     NAME ROSConDemo ${PAL_TRAIT_MONOLITHIC_DRIVEN_MODULE_TYPE}

Project/Gem/Include/ROSConDemo/ROSConDemoBus.h

@@ -1,3 +1,10 @@
+/*
+* 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
 

Project/Gem/Source/ApplePicker/GatheringRowComponent.cpp

@@ -0,0 +1,93 @@
+/*
+ * 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 "GatheringRowComponent.h"
+#include <AzCore/Component/TransformBus.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+
+namespace AppleKraken
+{
+    void GatheringRowComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<GatheringRowComponent, AZ::Component>()
+                ->Version(3)
+                ->Field("Start", &GatheringRowComponent::m_start)
+                ->Field("End", &GatheringRowComponent::m_end)
+                ->Field("PoseOffset", &GatheringRowComponent::m_poseOffset)
+                ->Field("TreeCount", &GatheringRowComponent::m_appleTreeCount);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<GatheringRowComponent>("Gathering Row Component", "Poses (points with orientation) suitable for apple gathering")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "AppleKraken")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &GatheringRowComponent::m_start, "Start", "Entity with the start pose")
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &GatheringRowComponent::m_end, "End", "Entity with the end pose")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GatheringRowComponent::m_poseOffset,
+                        "Offset",
+                        "Pose offset for each point (depends on robot)")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GatheringRowComponent::m_appleTreeCount,
+                        "Tree count",
+                        "How many trees are in the row")
+                    ->Attribute(AZ::Edit::Attributes::Min, 1);
+            }
+        }
+    }
+
+    void GatheringRowComponent::Activate()
+    {
+        ComputeGatheringPoses();
+        GatheringRowRequestBus::Handler::BusConnect();
+    }
+
+    void GatheringRowComponent::Deactivate()
+    {
+        GatheringRowRequestBus::Handler::BusDisconnect();
+        m_gatheringPoses.clear();
+    }
+
+    void GatheringRowComponent::ComputeGatheringPoses()
+    {
+        if (!m_start.IsValid() || !m_end.IsValid())
+        {
+            AZ_Error("GatheringRowComponent", false, "ComputeGatheringPoses: unable to proceed without both start and end entity set");
+            return;
+        }
+
+        if (m_appleTreeCount < 1)
+        {
+            AZ_Error("GatheringRowComponent", false, "ComputeGatheringPoses: unable to proceed with apple tree count less than 1");
+            return;
+        }
+
+        GatheringPoses allPoses;
+        AZ::Transform startPose;
+        AZ::Transform endPose;
+        AZ::TransformBus::EventResult(startPose, m_start, &AZ::TransformBus::Events::GetWorldTM);
+        AZ::TransformBus::EventResult(endPose, m_end, &AZ::TransformBus::Events::GetWorldTM);
+
+        // Simplification - we assume same orientations along the way
+        auto rowVector = endPose.GetTranslation() - startPose.GetTranslation();
+        for (int i = 0; i < m_appleTreeCount; ++i)
+        {
+            AZ::Transform gatheringPoint = startPose;
+            float scale = i / (m_appleTreeCount - 1);
+            gatheringPoint.SetTranslation(gatheringPoint.GetTranslation() + rowVector * scale);
+            m_gatheringPoses.push_back(gatheringPoint);
+        }
+    }
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/GatheringRowComponent.h

@@ -0,0 +1,45 @@
+/*
+ * 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 "GatheringRowRequests.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/EntityId.h>
+
+namespace AppleKraken
+{
+    //! Demo component wrapping information about gathering positions (single row of apple trees)
+    class GatheringRowComponent
+        : public AZ::Component
+        , public GatheringRowRequestBus::Handler
+    {
+    public:
+        AZ_COMPONENT(GatheringRowComponent, "{32987AAB-D275-40E0-B9BC-A8108522C055}", AZ::Component);
+        GatheringRowComponent() = default;
+        static void Reflect(AZ::ReflectContext* context);
+
+        //! First on the list is always the start pose, the last is the end pose
+        GatheringPoses GetGatheringPoses() const override
+        {
+            return m_gatheringPoses;
+        }
+
+    private:
+        void Activate() override;
+        void Deactivate() override;
+
+        void ComputeGatheringPoses();
+
+        AZ::EntityId m_start;
+        AZ::EntityId m_end;
+        AZ::Vector3 m_poseOffset = AZ::Vector3::CreateZero(); //!< Depends on robot / effector setting, exposed for experimenting
+        uint16_t m_appleTreeCount = 13;
+
+        GatheringPoses m_gatheringPoses;
+    };
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/GatheringRowRequests.h

@@ -0,0 +1,28 @@
+/*
+ * 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 "GatheringRowRequests.h"
+#include <AzCore/EBus/EBus.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/std/containers/vector.h>
+
+namespace AppleKraken
+{
+    using GatheringPoses = AZStd::vector<AZ::Transform>;
+
+    //! Demo component wrapping information about gathering positions (single row of apple trees)
+    class GatheringRowRequests : public AZ::EBusTraits
+    {
+    public:
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple;
+        virtual GatheringPoses GetGatheringPoses() const = 0;
+    };
+
+    using GatheringRowRequestBus = AZ::EBus<GatheringRowRequests>;
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -20,6 +20,7 @@
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/Shape.h>
 #include <AzFramework/Physics/SystemBus.h>
+
 namespace AppleKraken
 {
     //! Component for apple picking effector (manipulator)

Project/Gem/Source/ROSConDemoModule.cpp

@@ -1,5 +1,13 @@
+/*
+ * 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 "ApplePicker/ApplePickerComponent.h"
+#include "ApplePicker/GatheringRowComponent.h"
 #include "ApplePicker/KrakenEffectorComponent.h"
 #include "FruitStorage/FruitStorageComponent.h"
 #include "ROSConDemoSystemComponent.h"
@@ -21,6 +29,7 @@ namespace ROSConDemo
                 m_descriptors.end(),
                 { ROSConDemoSystemComponent::CreateDescriptor(),
                   AppleKraken::ApplePickerComponent::CreateDescriptor(),
+                  AppleKraken::GatheringRowComponent::CreateDescriptor(),
                   AppleKraken::KrakenEffectorComponent::CreateDescriptor(),
                   AppleKraken::FruitStorageComponent::CreateDescriptor() });
         }

Project/Gem/Source/ROSConDemoSystemComponent.cpp

@@ -1,8 +1,18 @@
+/*
+* 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 "ROSConDemoSystemComponent.h"
+#include "ApplePicker/GatheringRowRequests.h"
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
 
 namespace ROSConDemo
 {
@@ -56,17 +66,40 @@ namespace ROSConDemo
         }
     }
 
+    void ROSConDemoSystemComponent::ProcessGetPlanServiceCall(const GetPlanRequestPtr req, GetPlanResponsePtr resp)
+    {
+        AppleKraken::GatheringPoses results;
+        AppleKraken::GatheringRowRequestBus::BroadcastResult(results, &AppleKraken::GatheringRowRequests::GetGatheringPoses);
+        // TODO - get closest to startTransform
+        // auto startTransform = ROS2::ROS2Conversions::FromROS2Pose(req->start.pose);
+
+        for (auto result : results)
+        {
+            geometry_msgs::msg::PoseStamped stampedPose; // TODO - fill in header
+            stampedPose.pose = ROS2::ROS2Conversions::ToROS2Pose(result);
+            resp->plan.poses.push_back(stampedPose);
+        }
+    }
+
     void ROSConDemoSystemComponent::Init()
     {
     }
 
     void ROSConDemoSystemComponent::Activate()
-    {
+    {   // TODO - the service should probably only be created in Game Mode
         ROSConDemoRequestBus::Handler::BusConnect();
+        auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
+        m_pathPlanService = ros2Node->create_service<nav_msgs::srv::GetPlan>(
+            m_planTopic.c_str(),
+            [this](const GetPlanRequestPtr request, GetPlanResponsePtr response)
+            {
+                this->ProcessGetPlanServiceCall(request, response);
+            });
     }
 
     void ROSConDemoSystemComponent::Deactivate()
     {
+        m_pathPlanService.reset();
         ROSConDemoRequestBus::Handler::BusDisconnect();
     }
 } // namespace ROSConDemo

Project/Gem/Source/ROSConDemoSystemComponent.h

@@ -1,11 +1,23 @@
+/*
+* 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/Component.h>
 #include <ROSConDemo/ROSConDemoBus.h>
+#include <nav_msgs/srv/get_plan.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 namespace ROSConDemo
 {
+    using GetPlanRequestPtr = std::shared_ptr<nav_msgs::srv::GetPlan::Request>;
+    using GetPlanResponsePtr = std::shared_ptr<nav_msgs::srv::GetPlan::Response>;
+
     class ROSConDemoSystemComponent
         : public AZ::Component
         , protected ROSConDemoRequestBus::Handler
@@ -27,5 +39,11 @@ namespace ROSConDemo
         void Init() override;
         void Activate() override;
         void Deactivate() override;
+
+    private:
+        void ProcessGetPlanServiceCall(const GetPlanRequestPtr req, GetPlanResponsePtr resp);
+
+        AZStd::string m_planTopic = "get_gathering_plan";
+        rclcpp::Service<nav_msgs::srv::GetPlan>::SharedPtr m_pathPlanService;
     };
 } // namespace ROSConDemo

Project/Gem/roscondemo_files.cmake

@@ -7,9 +7,12 @@ set(FILES
         Source/ApplePicker/ApplePickerComponent.h
         Source/ApplePicker/ApplePickingNotifications.h
         Source/ApplePicker/ApplePickingRequests.h
-        Source/ApplePicker/PickingStructs.h
+        Source/ApplePicker/GatheringRowComponent.cpp
+        Source/ApplePicker/GatheringRowComponent.h
+        Source/ApplePicker/GatheringRowRequests.h
         Source/ApplePicker/KrakenEffectorComponent.cpp
         Source/ApplePicker/KrakenEffectorComponent.h
+        Source/ApplePicker/PickingStructs.h
         Source/FruitStorage/FruitStorageComponent.cpp
         Source/FruitStorage/FruitStorageComponent.h
         Source/FruitStorage/FruitStorageBus.h

Project/Levels/Main/Main.prefab

@@ -39,7 +39,8 @@
                     "Entity_[1176639161715]",
                     "Entity_[104676312987105]",
                     "Instance_[453183527714188]/ContainerEntity",
-                    "Entity_[1090499316769908]"
+                    "Entity_[1090499316769908]",
+                    "Entity_[1377679985301621]"
                 ]
             },
             "Component_[14409887347711167786]": {
@@ -767,7 +768,7 @@
                     "Controller": {
                         "Configuration": {
                             "Field of View": 55.0,
-                            "EditorEntityId": 15539094530770820237
+                            "EditorEntityId": 17843646596694970328
                         }
                     }
                 },
@@ -5731,6 +5732,162 @@
                 }
             }
         },
+        "Entity_[1377679985301621]": {
+            "Id": "Entity_[1377679985301621]",
+            "Name": "GatheringRow",
+            "Components": {
+                "Component_[12569860409072173675]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 12569860409072173675,
+                    "Parent Entity": "Entity_[1146574390643]",
+                    "Transform Data": {
+                        "Translate": [
+                            -59.84291458129883,
+                            35.47842788696289,
+                            0.0
+                        ]
+                    }
+                },
+                "Component_[12865825626512645118]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 12865825626512645118
+                },
+                "Component_[15471213523630498323]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 15471213523630498323
+                },
+                "Component_[18390545873450438881]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 18390545873450438881
+                },
+                "Component_[336629041609428677]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 336629041609428677
+                },
+                "Component_[5320963908403420676]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 5320963908403420676
+                },
+                "Component_[6506285051007944421]": {
+                    "$type": "GenericComponentWrapper",
+                    "Id": 6506285051007944421,
+                    "m_template": {
+                        "$type": "GatheringRowComponent",
+                        "Start": "Entity_[1377688575236213]",
+                        "End": "Entity_[1377697165170805]"
+                    }
+                },
+                "Component_[8164175847176343181]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 8164175847176343181,
+                    "Child Entity Order": [
+                        "Entity_[1377688575236213]",
+                        "Entity_[1377697165170805]"
+                    ]
+                },
+                "Component_[8225996335577294700]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 8225996335577294700
+                },
+                "Component_[9436431492391050860]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 9436431492391050860
+                }
+            }
+        },
+        "Entity_[1377688575236213]": {
+            "Id": "Entity_[1377688575236213]",
+            "Name": "Start",
+            "Components": {
+                "Component_[11538459141403005529]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 11538459141403005529,
+                    "Parent Entity": "Entity_[1377679985301621]"
+                },
+                "Component_[11947992469664550580]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 11947992469664550580
+                },
+                "Component_[12136844628209740403]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 12136844628209740403
+                },
+                "Component_[14199351808047054605]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 14199351808047054605
+                },
+                "Component_[14960584158294418045]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 14960584158294418045
+                },
+                "Component_[1845133092815959833]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 1845133092815959833
+                },
+                "Component_[5033738611993447155]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 5033738611993447155
+                },
+                "Component_[7192414286059315724]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 7192414286059315724
+                },
+                "Component_[8631506422829805841]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 8631506422829805841
+                }
+            }
+        },
+        "Entity_[1377697165170805]": {
+            "Id": "Entity_[1377697165170805]",
+            "Name": "End",
+            "Components": {
+                "Component_[14622380832334875767]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 14622380832334875767
+                },
+                "Component_[15625872885702800588]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 15625872885702800588
+                },
+                "Component_[16183695329271030129]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 16183695329271030129
+                },
+                "Component_[16456657311684002994]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 16456657311684002994,
+                    "Parent Entity": "Entity_[1377679985301621]",
+                    "Transform Data": {
+                        "Translate": [
+                            0.0,
+                            -38.903377532958984,
+                            0.0
+                        ]
+                    }
+                },
+                "Component_[17760302013238664821]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 17760302013238664821
+                },
+                "Component_[6079329122693101296]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 6079329122693101296
+                },
+                "Component_[6636293252330045092]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 6636293252330045092
+                },
+                "Component_[7890849669701597370]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 7890849669701597370
+                },
+                "Component_[8436891138138350551]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 8436891138138350551
+                }
+            }
+        },
         "Entity_[14042563791436142]": {
             "Id": "Entity_[14042563791436142]",
             "Name": "AssetsSelector",
@@ -7962,7 +8119,7 @@
                                 },
                                 "assetHint": "reflectionprobes/refprobmain__c4ee43e2-3ffe-4955-9dee-679ca04c2a53__iblspecularcm256.dds.streamingimage"
                             },
-                            "EntityId": 6491891658724700607
+                            "EntityId": 5861860565537541765
                         }
                     },
                     "useBakedCubemap": false,
@@ -18288,7 +18445,7 @@
                 {
                     "op": "replace",
                     "path": "/ContainerEntity/Components/Component_[5123579376316041771]/Transform Data/Translate/2",
-                    "value": -0.012486711144447327
+                    "value": -0.012486711144447328
                 },
                 {
                     "op": "replace",
@@ -18388,7 +18545,7 @@
                 {
                     "op": "replace",
                     "path": "/Entities/Entity_[453802196716717]/Components/Component_[12122484710959107959]/Controller/Configuration/EditorEntityId",
-                    "value": 11896589052357720660
+                    "value": 15234215949839940759
                 }
             ]
         },

README.md

@@ -75,7 +75,7 @@ During the step above, make sure to register the Gem in the engine:
 
 The additional packages need to be installed. Use the following command:
 
-`sudo apt install ros-${ROS_DISTRO}-vision-msgs ros-${ROS_DISTRO}-rmw-cyclonedds-cpp ros-${ROS_DISTRO}-cyclonedds`
+`sudo apt install ros-${ROS_DISTRO}-vision-msgs ros-${ROS_DISTRO}-nav-msgs ros-${ROS_DISTRO}-rmw-cyclonedds-cpp ros-${ROS_DISTRO}-cyclonedds`
 
 💡 ***Note:*** This is a dependency besides all the packages already required by the ROS 2 Gem.