ros2 sevice trigger handling, dependencies

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

Project/Gem/CMakeLists.txt

@@ -31,10 +31,14 @@ ly_add_target(
     FILES_CMAKE
         roscondemo_files.cmake
         ${pal_dir}/roscondemo_${PAL_PLATFORM_NAME_LOWERCASE}_files.cmake
+    PLATFORM_INCLUDE_FILES
+        ${CMAKE_CURRENT_LIST_DIR}/Platform/Common/${PAL_TRAIT_COMPILER_ID}/exceptions_${PAL_TRAIT_COMPILER_ID_LOWERCASE}.cmake
     INCLUDE_DIRECTORIES
         PUBLIC
             Include
     BUILD_DEPENDENCIES
+        PUBLIC
+            Gem::ROS2.Static
         PRIVATE
             AZ::AzGameFramework
             Gem::LmbrCentral.API
@@ -42,6 +46,8 @@ ly_add_target(
             Gem::Atom_AtomBridge.Static
 )
 
+target_depends_on_ros2_packages(ROSConDemo.Static std_srvs)
+
 ly_add_target(
     NAME ROSConDemo ${PAL_TRAIT_MONOLITHIC_DRIVEN_MODULE_TYPE}
     NAMESPACE Gem

Project/Gem/Platform/Common/Clang/exceptions_clang.cmake

@@ -0,0 +1,11 @@
+#
+# 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
+#
+
+set(LY_COMPILE_OPTIONS
+        PRIVATE
+            -fexceptions -frtti
+)

Project/Gem/Platform/Common/GCC/exceptions_gcc.cmake

@@ -0,0 +1,11 @@
+#
+# 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
+#
+
+set(LY_COMPILE_OPTIONS
+        PRIVATE
+            -fexceptions -frtti
+)

Project/Gem/Platform/Common/msvc/exceptions_msvc.cmake

@@ -0,0 +1,11 @@
+#
+# 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
+#
+
+set(LY_COMPILE_OPTIONS
+        PRIVATE
+            /EHsc /GR
+)

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -11,6 +11,11 @@
 #include <AzCore/EBus/Event.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Names.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+
+using namespace ROS2;
 
 #include <AtomLyIntegration/CommonFeatures/Mesh/MeshComponentBus.h>
 #include <AtomLyIntegration/CommonFeatures/Mesh/MeshComponentConstants.h>
@@ -45,9 +50,43 @@ namespace AppleKraken
         }
     } // namespace Internal
 
-    void ApplePickerComponent::StartAutomatedOperation()
+    bool ApplePickerComponent::IsBusy() const
     {
         if (!m_currentAppleTasks.empty())
+        {   // busy - still has tasks in queue
+            return true;
+        }
+
+        // There are no apple tasks, but the effector might not be idle or prepared yet
+        PickingState pickingState;
+        ApplePickingRequestBus::EventResult(pickingState, m_effectorEntityId, &ApplePickingRequests::GetEffectorState);
+        if (pickingState.m_effectorState != EffectorState::IDLE && pickingState.m_effectorState != EffectorState::PREPARED)
+        {   // Effector is not ready - still transitioning to a state. This should be a rare occurrence.
+            AZ_Warning("ApplePicker", false, "Task queue empty but apple picker is busy since the effector is working");
+            return true;
+        }
+        return false;
+    }
+
+    void ApplePickerComponent::ProcessTriggerServiceCall(const TriggerRequest req, TriggerResponse resp)
+    {
+        // TODO - also, perhaps add a check whether Kraken is in gathering position, immobile etc.
+        if (IsBusy())
+        {
+            resp->success = false;
+            resp->message = "Unable to accept request - apple picker is currently busy";
+            return;
+        }
+
+        resp->success = true;
+        resp->message = "Command accepted, picking operation started";
+        StartAutomatedOperation();
+        return;
+    }
+
+    void ApplePickerComponent::StartAutomatedOperation()
+    {
+        if (IsBusy())
         {
             AZ_Error("ApplePicker", false, "Tasks still in progress for current picking!");
             return;
@@ -67,12 +106,6 @@ namespace AppleKraken
     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()
@@ -92,10 +125,21 @@ namespace AppleKraken
         m_effectorEntityId = GetEntityId(); // TODO - remove this once we expose this field
         ApplePickingNotificationBus::Handler::BusConnect();
         AZ::TickBus::Handler::BusConnect();
+
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        auto robotNamespace = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity())->GetNamespace();
+        auto topic = ROS2Names::GetNamespacedName(robotNamespace, m_triggerServiceTopic);
+        m_triggerService = ros2Node->create_service<std_srvs::srv::Trigger>(
+            m_triggerServiceTopic.c_str(),
+            [this](const TriggerRequest request, TriggerResponse response)
+            {
+                this->ProcessTriggerServiceCall(request, response);
+            });
     }
 
     void ApplePickerComponent::Deactivate()
     {
+        m_triggerService.reset();
         AZ::TickBus::Handler::BusDisconnect();
         ApplePickingNotificationBus::Handler::BusDisconnect();
     }
@@ -115,6 +159,11 @@ namespace AppleKraken
         }
     }
 
+    void ApplePickerComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC("ROS2Frame"));
+    }
+
     void ApplePickerComponent::EffectorReadyForPicking()
     {
         PickNextApple();

Project/Gem/Source/ApplePicker/ApplePickerComponent.h

@@ -11,9 +11,14 @@
 #include "ApplePickingRequests.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
+#include <std_srvs/srv/trigger.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 namespace AppleKraken
 {
+    using TriggerRequest = std::shared_ptr<std_srvs::srv::Trigger::Request>;
+    using TriggerResponse = std::shared_ptr<std_srvs::srv::Trigger::Response>;
+
     //! Demo component handling orchestration of apple picking
     class ApplePickerComponent
         : public AZ::Component
@@ -24,6 +29,7 @@ namespace AppleKraken
         AZ_COMPONENT(ApplePickerComponent, "{E9E83A4A-31A4-4E7A-AF88-7565AC8B9F27}", AZ::Component);
         ApplePickerComponent() = default;
         static void Reflect(AZ::ReflectContext* context);
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
 
         //! Detect and pick all apples in manipulator range.
         void StartAutomatedOperation();
@@ -44,8 +50,13 @@ namespace AppleKraken
         void AppleRetrieved() override;
         void PickingFailed(const AZStd::string& reason) override;
 
+        bool IsBusy() const;
         void PickNextApple();
         void QueryEnvironmentForAllApplesInBox(const AZ::Obb& globalBox);
+        void ProcessTriggerServiceCall(const TriggerRequest req, TriggerResponse resp);
+
+        AZStd::string m_triggerServiceTopic = "trigger_apple_gathering";
+        rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_triggerService;
 
         AZ::EntityId m_effectorEntityId;
         AZ::Obb m_gatheringArea;

README.md

@@ -50,6 +50,17 @@ This will compile after some time and binaries will be available in the project
 
 For a complete tutorial on project configuration, see [Creating Projects Using the Command Line Interface](https://o3de.org/docs/welcome-guide/create/creating-projects-using-cli/) in the documentation.
 
+## Triggering Apple Gathering
+
+Check available services in a terminal using this command:
+- `ros2 service list`
+
+If your simulation is running, you should be able to see the apple gathering service listed there.
+- It could be named `/trigger_apple_gathering`.
+
+If Apple Kraken is in position, you can trigger apple gathering with a terminal command:
+- `ros2 service call /trigger_apple_gathering std_srvs/srv/Trigger`
+
 ## License
 
 For terms please see the LICENSE*.TXT files at the root of this distribution.