SimulationInterfacesROS2 - added action server for SimulateSteps (#861)

* Added handler for new action - SimulateSteps

*added new base class for the actions
*extended base interface for all services, actions and topics handlers
*fixed some minor bugs
*fixed typos
*updated gem.json
*updated logs msgs and comments

---------

Signed-off-by: Patryk Antosz <[email protected]>
Co-authored-by: Michał Pełka <[email protected]>
Co-authored-by: Norbert Prokopiuk <[email protected]>
Co-authored-by: Jan Hanca <[email protected]>

Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h

@@ -31,5 +31,6 @@ namespace SimulationInterfaces
     inline constexpr const char* SimulationInterfacesRequestsTypeId = "{6818E5E3-BBF5-41BD-96BB-7AF57CCC7528}";
     inline constexpr const char* SimulationManagerRequestsTypeId = "{056477BA-8153-4901-9401-0146A5E3E9ED}";
     inline constexpr const char* SimulationFeaturesAggregatorRequestsTypeId = "{099fd08b-b0e2-4705-9c35-cc09c8e45076}";
+    inline constexpr const char* SimulationManagerNotificationsTypeId = "{0201067b-9d52-4ab7-9a45-284287f53b00}";
 
 } // namespace SimulationInterfaces

Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h

@@ -30,7 +30,18 @@ namespace SimulationInterfaces
 
         //! Step the simulation by a number of steps
         //! expect always to succeed
-        virtual void StepSimulation(AZ::u32 steps) = 0;
+        virtual void StepSimulation(AZ::u64 steps) = 0;
+
+        //! Check whether the simulation is paused or not
+        //! @return boolean value indicating the pause state of the simulation
+        virtual bool IsSimulationPaused() const = 0;
+
+        //! Cancel executing the simulation steps, if there are remaining steps left
+        virtual void CancelStepSimulation() = 0;
+
+        //! Check if the SimulationSteps is active
+        //! @return boolean value indicating if the SimulationSteps is active
+        virtual bool IsSimulationStepsActive() const = 0;
     };
 
     class SimulationMangerRequestBusTraits : public AZ::EBusTraits
@@ -46,4 +57,27 @@ namespace SimulationInterfaces
     using SimulationManagerRequestBus = AZ::EBus<SimulationManagerRequests, SimulationMangerRequestBusTraits>;
     using SimulationManagerRequestBusInterface = AZ::Interface<SimulationManagerRequests>;
 
+    class SimulationManagerNotifications
+    {
+    public:
+        AZ_RTTI(SimulationManagerNotifications, SimulationManagerNotificationsTypeId);
+        virtual ~SimulationManagerNotifications() = default;
+
+        //! Notify about simulation step finish
+        //! @param remainingSteps - remaining steps to pause simulation
+        virtual void OnSimulationStepFinish(const AZ::u64 remainingSteps) = 0;
+    };
+
+    class SimulationMangerNotificationsBusTraits : public AZ::EBusTraits
+    {
+    public:
+        //////////////////////////////////////////////////////////////////////////
+        // EBusTraits overrides
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single;
+        //////////////////////////////////////////////////////////////////////////
+    };
+
+    using SimulationManagerNotificationsBus = AZ::EBus<SimulationManagerNotifications, SimulationMangerNotificationsBusTraits>;
+
 } // namespace SimulationInterfaces

Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp

@@ -85,6 +85,27 @@ namespace SimulationInterfaces
         SimulationManagerRequestBus::Handler::BusDisconnect();
     }
 
+    bool SimulationManager::IsSimulationPaused() const
+    {
+        return m_isSimulationPaused;
+    }
+
+    bool SimulationManager::IsSimulationStepsActive() const
+    {
+        return m_simulationFinishEvent.IsConnected();
+    }
+
+    void SimulationManager::CancelStepSimulation()
+    {
+        if (m_simulationFinishEvent.IsConnected())
+        {
+            m_simulationFinishEvent.Disconnect();
+            SetSimulationPaused(true);
+            m_numberOfPhysicsSteps = 0;
+        }
+    }
+
+
     void SimulationManager::SetSimulationPaused(bool paused)
     {
         // get az physics system
@@ -97,11 +118,16 @@ namespace SimulationInterfaces
         {
             AZ_Assert(scene, "Physics scene is not available");
             scene->SetEnabled(!paused);
+            m_isSimulationPaused = paused;
         }
     }
 
-    void SimulationManager::StepSimulation(AZ::u32 steps)
+    void SimulationManager::StepSimulation(AZ::u64 steps)
     {
+        if (steps == 0)
+        {
+            return;
+        }
         m_numberOfPhysicsSteps = steps;
 
         // install handler
@@ -110,6 +136,7 @@ namespace SimulationInterfaces
             {
                 m_numberOfPhysicsSteps--;
                 AZ_Printf("SimulationManager", "Physics simulation step finished. Remaining steps: %d", m_numberOfPhysicsSteps);
+                SimulationManagerNotificationsBus::Broadcast(&SimulationManagerNotifications::OnSimulationStepFinish, m_numberOfPhysicsSteps);
                 if (m_numberOfPhysicsSteps <= 0)
                 {
                     SetSimulationPaused(true);

Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h

@@ -42,8 +42,13 @@ namespace SimulationInterfaces
 
     protected:
         void SetSimulationPaused(bool paused) override;
-        void StepSimulation(AZ::u32 steps) override;
-        uint32_t m_numberOfPhysicsSteps = 0;
+        void StepSimulation(AZ::u64 steps) override;
+        bool IsSimulationPaused() const override;
+        void CancelStepSimulation() override;
+        bool IsSimulationStepsActive() const override;
+
+        bool m_isSimulationPaused = false;
+        uint64_t m_numberOfPhysicsSteps = 0;
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent;
     };
 } // namespace SimulationInterfaces

Gems/SimulationInterfacesROS2/Code/CMakeLists.txt

@@ -30,6 +30,7 @@ ly_add_target(
     BUILD_DEPENDENCIES
         INTERFACE
            AZ::AzCore
+           Gem::SimulationInterfaces.API
 )
 
 # The ${gem_name}.Private.Object target is an internal target
@@ -55,7 +56,7 @@ ly_add_target(
             AZ::AzFramework
 )
 
-target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces)
+target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces rclcpp_action)
 
 # Here add ${gem_name} target, it depends on the Private Object library and Public API interface
 ly_add_target(

Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h

@@ -0,0 +1,127 @@
+/*
+ * 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/std/functional.h>
+#include <Interfaces/IROS2HandlerBase.h>
+#include <Utils/RegistryUtils.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <simulation_interfaces/msg/simulator_features.hpp>
+
+namespace SimulationInterfacesROS2
+{
+    //! Base for each ROS 2 action server handler, forces declaration of features provided by the server
+    //! combined information along all ROS 2 handlers gives information about simulation features
+    //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
+    using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures;
+    template<typename RosActionType>
+    class ROS2ActionBase : public virtual IROS2HandlerBase
+    {
+    public:
+        using Result = typename RosActionType::Result;
+        using Feedback = typename RosActionType::Feedback;
+        using Goal = typename RosActionType::Goal;
+        using ActionHandle = typename rclcpp_action::Server<RosActionType>::SharedPtr;
+        using GoalHandle = rclcpp_action::ServerGoalHandle<RosActionType>;
+
+        virtual ~ROS2ActionBase() = default;
+
+        void Initialize(rclcpp::Node::SharedPtr& node) override
+        {
+            CreateAction(node);
+        }
+
+    protected:
+        //! This function is called when the action is cancelled
+        virtual void CancelGoal(std::shared_ptr<Result> result)
+        {
+            AZ_Assert(m_goalHandle, "Invalid goal handle!");
+            if (m_goalHandle && m_goalHandle->is_canceling())
+            {
+                m_goalHandle->canceled(result);
+                m_goalHandle.reset();
+            }
+        }
+
+        //! This function is called when the action successfully finished
+        virtual void GoalSuccess(std::shared_ptr<Result> result)
+        {
+            AZ_Assert(m_goalHandle, "Invalid goal handle!");
+            if (m_goalHandle && (m_goalHandle->is_executing() || m_goalHandle->is_canceling()))
+            {
+                m_goalHandle->succeed(result);
+                m_goalHandle.reset();
+            }
+        }
+
+        //! This function is called on demand, based on the action server implementation, to share the action progress
+        virtual void PublishFeedback(std::shared_ptr<Feedback> feedback)
+        {
+            AZ_Assert(m_goalHandle, "Invalid goal handle!");
+            if (m_goalHandle && m_goalHandle->is_executing())
+            {
+                m_goalHandle->publish_feedback(feedback);
+            }
+        }
+
+        //! This function check if the server is ready to handle new goal
+        virtual bool IsReadyForExecution() const
+        {
+            // Has no goal handle yet - can be accepted.
+            if (!m_goalHandle)
+            {
+                return true;
+            }
+            // Accept if the previous one is in a terminal state.
+            return !(m_goalHandle->is_active() || m_goalHandle->is_executing() || m_goalHandle->is_canceling());
+        }
+
+        //! This function is called when the new goal receives
+        virtual rclcpp_action::GoalResponse GoalReceivedCallback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const Goal> goal) = 0;
+
+        //! This function is called when the currently executed goal is cancelled
+        virtual rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr<GoalHandle> goal_handle) = 0;
+
+        //! This function is called when the newly received goal is accepted
+        virtual void GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goal_handle) = 0;
+
+        //! return features id defined by the handler, ids must follow the definition inside standard:
+        //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
+        AZStd::unordered_set<AZ::u8> GetProvidedFeatures() override
+        {
+            return {};
+        };
+
+        std::shared_ptr<GoalHandle> m_goalHandle;
+
+    private:
+        void CreateAction(rclcpp::Node::SharedPtr& node)
+        {
+            // Get the action name from the type name
+            AZStd::string actionName = RegistryUtilities::GetName(GetTypeName());
+
+            if (actionName.empty())
+            {
+                // if the action name is empty, use the default name
+                actionName = GetDefaultName();
+            }
+
+            const std::string actionNameStr{ actionName.c_str(), actionName.size() };
+            m_actionHandle = rclcpp_action::create_server<RosActionType>(
+                node,
+                actionNameStr,
+                AZStd::bind(&ROS2ActionBase::GoalReceivedCallback, this, AZStd::placeholders::_1, AZStd::placeholders::_2),
+                AZStd::bind(&ROS2ActionBase::GoalCancelledCallback, this, AZStd::placeholders::_1),
+                AZStd::bind(&ROS2ActionBase::GoalAcceptedCallback, this, AZStd::placeholders::_1));
+        }
+
+        ActionHandle m_actionHandle;
+    };
+} // namespace SimulationInterfacesROS2

Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp

@@ -0,0 +1,121 @@
+/*
+ * 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 "SimulateStepsActionServerHandler.h"
+
+namespace SimulationInterfacesROS2
+{
+
+    void SimulateStepsActionServerHandler::Initialize(rclcpp::Node::SharedPtr& node)
+    {
+        ROS2ActionBase::Initialize(node);
+        SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusConnect();
+    }
+
+    SimulateStepsActionServerHandler::~SimulateStepsActionServerHandler()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusDisconnect();
+    }
+
+    AZStd::unordered_set<AZ::u8> SimulateStepsActionServerHandler::GetProvidedFeatures()
+    {
+        return AZStd::unordered_set<AZ::u8>{ SimulationFeatures::STEP_SIMULATION_ACTION };
+    }
+
+    AZStd::string_view SimulateStepsActionServerHandler::GetTypeName() const
+    {
+        return "SimulateSteps";
+    }
+
+    AZStd::string_view SimulateStepsActionServerHandler::GetDefaultName() const
+    {
+        return "simulate_steps";
+    }
+
+    rclcpp_action::GoalResponse SimulateStepsActionServerHandler::GoalReceivedCallback(
+        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const Goal> goal)
+    {
+        if (!IsReadyForExecution())
+        {
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+
+        m_goalSteps = goal->steps;
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    rclcpp_action::CancelResponse SimulateStepsActionServerHandler::GoalCancelledCallback(
+        [[maybe_unused]] const std::shared_ptr<GoalHandle> goal_handle)
+    {
+        SimulationInterfaces::SimulationManagerRequestBus::Broadcast(
+            &SimulationInterfaces::SimulationManagerRequests::CancelStepSimulation);
+        m_isCancelled = true;
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void SimulateStepsActionServerHandler::GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goal_handle)
+    {
+        m_goalHandle = goal_handle;
+
+        bool isPaused = false;
+        SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult(
+            isPaused, &SimulationInterfaces::SimulationManagerRequests::IsSimulationPaused);
+        if (!isPaused)
+        {
+            //! If simulation is not paused then action should not be processed and action server should return Result with result code set
+            //! to the RESULT_OPERATION_FAILED value, according to the documentation:
+            //! https://github.com/ros-simulation/simulation_interfaces/blob/main/action/SimulateSteps.action
+            auto result = std::make_shared<Result>();
+            result->result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
+            result->result.error_message = "Request cannot be processed - simulation has to be paused. Action will be aborted.";
+            m_goalHandle->abort(result);
+            m_goalHandle.reset();
+            return;
+        }
+        m_isCancelled = false;
+        AZ::TickBus::Handler::BusConnect();
+        SimulationInterfaces::SimulationManagerRequestBus::Broadcast(
+            &SimulationInterfaces::SimulationManagerRequests::StepSimulation, m_goalSteps);
+    }
+
+    void SimulateStepsActionServerHandler::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (m_isCancelled)
+        {
+            auto result = std::make_shared<Result>();
+            result->result.error_message = "Action has been cancelled.";
+            result->result.result = simulation_interfaces::msg::Result::RESULT_OK;
+            CancelGoal(result);
+            AZ::TickBus::Handler::BusDisconnect();
+            return;
+        }
+
+        // If SimulationSteps is active then it means that SimulationManagerRequestBus::Handler is busy now with a previous request
+        bool isActive = true;
+        SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult(
+            isActive, &SimulationInterfaces::SimulationManagerRequests::IsSimulationStepsActive);
+        if (!isActive)
+        {
+            auto result = std::make_shared<Result>();
+            result->result.error_message = "Action finished.";
+            result->result.result = simulation_interfaces::msg::Result::RESULT_OK;
+            GoalSuccess(result);
+            AZ::TickBus::Handler::BusDisconnect();
+        }
+    }
+
+    void SimulateStepsActionServerHandler::OnSimulationStepFinish(const AZ::u64 remainingSteps)
+    {
+        auto feedback = std::make_shared<Feedback>();
+        feedback->remaining_steps = remainingSteps;
+        feedback->completed_steps = m_goalSteps - remainingSteps;
+        PublishFeedback(feedback);
+    }
+
+} // namespace SimulationInterfacesROS2

Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h

@@ -0,0 +1,50 @@
+/*
+ * 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 "ROS2ActionBase.h"
+#include <AzCore/Component/TickBus.h>
+#include <SimulationInterfaces/SimulationMangerRequestBus.h>
+#include <simulation_interfaces/action/simulate_steps.hpp>
+
+namespace SimulationInterfacesROS2
+{
+
+    class SimulateStepsActionServerHandler
+        : public ROS2ActionBase<simulation_interfaces::action::SimulateSteps>
+        , public AZ::TickBus::Handler
+        , public SimulationInterfaces::SimulationManagerNotificationsBus::Handler
+    {
+    public:
+        ~SimulateStepsActionServerHandler();
+
+        // IROS2HandlerBase overrides
+        AZStd::unordered_set<AZ::u8> GetProvidedFeatures() override;
+        AZStd::string_view GetTypeName() const override;
+        AZStd::string_view GetDefaultName() const override;
+        void Initialize(rclcpp::Node::SharedPtr& node) override;
+
+        // ROS2ActionBase<simulation_interfaces::action::SimulateSteps> overrides
+        rclcpp_action::GoalResponse GoalReceivedCallback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const Goal> goal) override;
+        rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr<GoalHandle> goal_handle) override;
+        void GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goal_handle) override;
+
+    protected:
+        // AZ::TickBus::Handler overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        // SimulationInterfaces::SimulationManagerNotificationsBus::Handler
+        void OnSimulationStepFinish(const AZ::u64 remainingSteps) override;
+
+    private:
+        AZ::u64 m_goalSteps{ 0 };
+        bool m_isCancelled{ false };
+    };
+
+} // namespace SimulationInterfacesROS2

Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp

@@ -7,14 +7,15 @@
  */
 
 #include "SimulationInterfacesROS2SystemComponent.h"
-#include "Services/ROS2HandlerBaseClass.h"
-#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h"
+
+#include <Actions/SimulateStepsActionServerHandler.h>
 #include <AzCore/std/string/string.h>
+#include <Services/ROS2ServiceBase.h>
+#include <SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h>
 
 #include <ROS2/ROS2Bus.h>
 #include <SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h>
 
-#include "Utils/RegistryUtils.h"
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/std/smart_ptr/make_shared.h>
 
@@ -27,10 +28,10 @@ namespace SimulationInterfacesROS2
         void RegisterInterface(
             AZStd::unordered_map<AZStd::string, AZStd::shared_ptr<IROS2HandlerBase>>& interfacesMap, rclcpp::Node::SharedPtr ros2Node)
         {
-            AZStd::shared_ptr service = AZStd::make_shared<T>();
-            service->CreateService(ros2Node);
-            interfacesMap[service->GetTypeName()] = AZStd::move(service);
-            service.reset();
+            AZStd::shared_ptr handler = AZStd::make_shared<T>();
+            handler->Initialize(ros2Node);
+            interfacesMap[handler->GetTypeName()] = AZStd::move(handler);
+            handler.reset();
         };
     } // namespace
 
@@ -85,24 +86,25 @@ namespace SimulationInterfacesROS2
         RegisterInterface<SetEntityStateServiceHandler>(m_availableRos2Interface, ros2Node);
         RegisterInterface<SpawnEntityServiceHandler>(m_availableRos2Interface, ros2Node);
         RegisterInterface<GetSimulationFeaturesServiceHandler>(m_availableRos2Interface, ros2Node);
+        RegisterInterface<SimulateStepsActionServerHandler>(m_availableRos2Interface, ros2Node);
     }
 
     void SimulationInterfacesROS2SystemComponent::Deactivate()
     {
         SimulationInterfacesROS2RequestBus::Handler::BusDisconnect();
 
-        for (auto& [serviceType, service] : m_availableRos2Interface)
+        for (auto& [handlerType, handler] : m_availableRos2Interface)
         {
-            service.reset();
+            handler.reset();
         }
     }
 
     AZStd::unordered_set<AZ::u8> SimulationInterfacesROS2SystemComponent::GetSimulationFeatures()
     {
         AZStd::unordered_set<AZ::u8> result;
-        for (auto& [serviceType, serviceHandler] : m_availableRos2Interface)
+        for (auto& [handlerType, handler] : m_availableRos2Interface)
         {
-            auto features = serviceHandler->GetProvidedFeatures();
+            auto features = handler->GetProvidedFeatures();
             result.insert(features.begin(), features.end());
         }
         return result;

Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h

@@ -17,7 +17,7 @@
 #include "Services/GetEntityStateServiceHandler.h"
 #include "Services/GetSimulationFeaturesServiceHandler.h"
 #include "Services/GetSpawnablesServiceHandler.h"
-#include "Services/ROS2HandlerBaseClass.h"
+#include "Services/ROS2ServiceBase.h"
 #include "Services/SetEntityStateServiceHandler.h"
 #include "Services/SpawnEntityServiceHandler.h"
 #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h"

Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h

@@ -9,6 +9,7 @@
 #pragma once
 #include <AzCore/std/containers/unordered_set.h>
 #include <AzCore/std/string/string_view.h>
+#include <rclcpp/rclcpp.hpp>
 
 namespace SimulationInterfacesROS2
 {
@@ -20,5 +21,6 @@ namespace SimulationInterfacesROS2
         virtual AZStd::unordered_set<AZ::u8> GetProvidedFeatures() = 0;
         virtual AZStd::string_view GetTypeName() const = 0;
         virtual AZStd::string_view GetDefaultName() const = 0;
+        virtual void Initialize(rclcpp::Node::SharedPtr& node) = 0;
     };
 } // namespace SimulationInterfacesROS2

Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp

@@ -7,7 +7,6 @@
  */
 
 #include "DeleteEntityServiceHandler.h"
-#include <AzCore/std/optional.h>
 #include <AzCore/std/string/string.h>
 #include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
 

Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h

@@ -8,15 +8,14 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/delete_entity.hpp>
 
 namespace SimulationInterfacesROS2
 {
 
-    class DeleteEntityServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::DeleteEntity>
+    class DeleteEntityServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::DeleteEntity>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp

@@ -7,9 +7,9 @@
  */
 
 #include "GetEntitiesServiceHandler.h"
-#include "Utils/Utils.h"
 #include <AzFramework/Physics/ShapeConfiguration.h>
 #include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
+#include <Utils/Utils.h>
 
 namespace SimulationInterfacesROS2
 {

Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h

@@ -8,14 +8,13 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/get_entities.hpp>
 
 namespace SimulationInterfacesROS2
 {
-    class GetEntitiesServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::GetEntities>
+    class GetEntitiesServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::GetEntities>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp

@@ -7,11 +7,10 @@
  */
 
 #include "GetEntitiesStatesServiceHandler.h"
-#include "Utils/Utils.h"
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Conversions.h>
 #include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
-#include <std_msgs/msg/header.hpp>
+#include <Utils/Utils.h>
 
 namespace SimulationInterfacesROS2
 {
@@ -70,15 +69,13 @@ namespace SimulationInterfacesROS2
             [](const auto& pair)
             {
                 const SimulationInterfaces::EntityState& entityState = pair.second;
-                simulation_interfaces::msg::EntityState simulationInterfaceEntityState;
-                std_msgs::msg::Header header;
-                header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
-                header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name();
-                simulationInterfaceEntityState.header = header;
-                simulationInterfaceEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose);
-                simulationInterfaceEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear);
-                simulationInterfaceEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular);
-                return simulationInterfaceEntityState;
+                simulation_interfaces::msg::EntityState simulationInterfacesEntityState;
+                simulationInterfacesEntityState.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
+                simulationInterfacesEntityState.header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name();
+                simulationInterfacesEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose);
+                simulationInterfacesEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear);
+                simulationInterfacesEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular);
+                return simulationInterfacesEntityState;
             });
         response.entities = stdEntities;
         response.states = stdEntityStates;

Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h

@@ -8,15 +8,14 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/get_entities_states.hpp>
 
 namespace SimulationInterfacesROS2
 {
 
-    class GetEntitiesStatesServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::GetEntitiesStates>
+    class GetEntitiesStatesServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::GetEntitiesStates>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp

@@ -7,7 +7,6 @@
  */
 
 #include "GetEntityStateServiceHandler.h"
-#include <AzCore/std/optional.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Conversions.h>
 #include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>

Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h

@@ -8,15 +8,14 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/get_entity_state.hpp>
 
 namespace SimulationInterfacesROS2
 {
 
-    class GetEntityStateServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::GetEntityState>
+    class GetEntityStateServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::GetEntityState>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp

@@ -7,11 +7,10 @@
  */
 
 #include "GetSimulationFeaturesServiceHandler.h"
-#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h"
 #include <AzCore/base.h>
 #include <AzCore/std/containers/unordered_set.h>
-#include <AzCore/std/containers/vector.h>
 #include <SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h>
+#include <SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h>
 
 namespace SimulationInterfacesROS2
 {

Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h

@@ -7,16 +7,15 @@
  */
 
 #pragma once
-#include "Services/ROS2HandlerBaseClass.h"
+
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
-#include <simulation_interfaces/srv/delete_entity.hpp>
-#include <simulation_interfaces/srv/detail/get_simulator_features__struct.hpp>
+#include <simulation_interfaces/srv/get_simulator_features.hpp>
 
 namespace SimulationInterfacesROS2
 {
 
-    class GetSimulationFeaturesServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::GetSimulatorFeatures>
+    class GetSimulationFeaturesServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::GetSimulatorFeatures>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h

@@ -8,15 +8,14 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/get_spawnables.hpp>
 
 namespace SimulationInterfacesROS2
 {
 
-    class GetSpawnablesServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::GetSpawnables>
+    class GetSpawnablesServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::GetSpawnables>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h → Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h

@@ -8,31 +8,57 @@
 
 #pragma once
 
-#include "Interfaces/IROS2HandlerBase.h"
-#include "Utils/RegistryUtils.h"
 #include <AzCore/std/containers/unordered_set.h>
 #include <AzCore/std/optional.h>
+#include <Interfaces/IROS2HandlerBase.h>
+#include <Utils/RegistryUtils.h>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/service.hpp>
 #include <simulation_interfaces/msg/simulator_features.hpp>
+
 namespace SimulationInterfacesROS2
 {
-    //! base for each ros2 service handler, forces declaration of features provided by the handler
-    //! combined informations along all ROS 2 handlers gives information about simulation features
+    //! Base for each ROS 2 service handler, forces declaration of features provided by the handler
+    //! combined information along all ROS 2 handlers gives information about simulation features
     //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
     using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures;
     template<typename RosServiceType>
-    class ROS2HandlerBase : public virtual IROS2HandlerBase
+    class ROS2ServiceBase : public virtual IROS2HandlerBase
     {
     public:
         using Request = typename RosServiceType::Request;
         using Response = typename RosServiceType::Response;
         using ServiceHandle = std::shared_ptr<rclcpp::Service<RosServiceType>>;
-        virtual ~ROS2HandlerBase() = default;
+        virtual ~ROS2ServiceBase() = default;
+
+        void Initialize(rclcpp::Node::SharedPtr& node) override
+        {
+            CreateService(node);
+        }
+
+        void SendResponse(Response response)
+        {
+            AZ_Assert(m_serviceHandle, "Failed to get m_serviceHandle");
+            AZ_Assert(m_lastRequestHeader, "Failed to get last request header ptr");
+            m_serviceHandle->send_response(*m_lastRequestHeader, response);
+        }
+
+    protected:
+        //! This function is called when a service request is received.
+        virtual AZStd::optional<Response> HandleServiceRequest(const std::shared_ptr<rmw_request_id_t> header, const Request& request) = 0;
+
+        //! return features id defined by the handler, ids must follow the definition inside standard:
+        //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
+        AZStd::unordered_set<AZ::u8> GetProvidedFeatures() override
+        {
+            return {};
+        };
+
+    private:
         void CreateService(rclcpp::Node::SharedPtr& node)
         {
             // get the service name from the type name
-            AZStd::string serviceName = RegistryUtilities::GetServiceName(GetTypeName());
+            AZStd::string serviceName = RegistryUtilities::GetName(GetTypeName());
 
             if (serviceName.empty())
             {
@@ -59,25 +85,6 @@ namespace SimulationInterfacesROS2
                 });
         }
 
-        void SendResponse(Response response)
-        {
-            AZ_Assert(m_serviceHandle, "Failed to get m_serviceHandle");
-            AZ_Assert(m_lastRequestHeader, "Failed to get last request header ptr");
-            m_serviceHandle->send_response(*m_lastRequestHeader, response);
-        }
-
-    protected:
-        //! This function is called when a service request is received.
-        virtual AZStd::optional<Response> HandleServiceRequest(const std::shared_ptr<rmw_request_id_t> header, const Request& request) = 0;
-
-        //! return features id defined by the handler, ids must follow the definition inside standard:
-        //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
-        AZStd::unordered_set<AZ::u8> GetProvidedFeatures() override
-        {
-            return {};
-        };
-
-    private:
         std::shared_ptr<rmw_request_id_t> m_lastRequestHeader = nullptr;
         ServiceHandle m_serviceHandle;
     };

Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp

@@ -7,7 +7,6 @@
  */
 
 #include "SetEntityStateServiceHandler.h"
-#include <AzCore/std/optional.h>
 #include <ROS2/Utilities/ROS2Conversions.h>
 #include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
 
@@ -39,7 +38,6 @@ namespace SimulationInterfacesROS2
             const auto& failedResult = outcome.GetError();
             response.result.result = aznumeric_cast<uint8_t>(failedResult.error_code);
             response.result.error_message = failedResult.error_string.c_str();
-            return response;
         }
 
         return response;

Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h

@@ -8,15 +8,14 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/set_entity_state.hpp>
 
 namespace SimulationInterfacesROS2
 {
 
-    class SetEntityStateServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::SetEntityState>
+    class SetEntityStateServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::SetEntityState>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp

@@ -7,8 +7,6 @@
  */
 
 #include "SpawnEntityServiceHandler.h"
-#include "Services/ROS2HandlerBaseClass.h"
-#include <AzCore/std/optional.h>
 #include <AzFramework/Physics/ShapeConfiguration.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Conversions.h>

Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h

@@ -8,14 +8,13 @@
 
 #pragma once
 
-#include "Services/ROS2HandlerBaseClass.h"
+#include "ROS2ServiceBase.h"
 #include <AzCore/std/string/string_view.h>
-#include <rclcpp/rclcpp.hpp>
 #include <simulation_interfaces/srv/spawn_entity.hpp>
 
 namespace SimulationInterfacesROS2
 {
-    class SpawnEntityServiceHandler : public ROS2HandlerBase<simulation_interfaces::srv::SpawnEntity>
+    class SpawnEntityServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::SpawnEntity>
     {
     public:
         AZStd::string_view GetTypeName() const override

Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp

@@ -12,7 +12,7 @@
 
 namespace SimulationInterfacesROS2::RegistryUtilities
 {
-    AZStd::string GetServiceName(AZStd::string serviceType)
+    AZStd::string GetName(AZStd::string serviceType)
     {
         AZ::SettingsRegistryInterface* settingsRegistry = AZ::SettingsRegistry::Get();
         AZ_Assert(settingsRegistry, "Settings Registry is not available");

Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h

@@ -16,5 +16,5 @@ namespace SimulationInterfacesROS2::RegistryUtilities
 
     //! Gets name of the service with defined type form settings registry
     //! @return string with service name, if setting registry doesn't exits returns empty string
-    [[nodiscard]] AZStd::string GetServiceName(AZStd::string serviceType);
+    [[nodiscard]] AZStd::string GetName(AZStd::string serviceType);
 } // namespace SimulationInterfacesROS2::RegistryUtilities

Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h

@@ -42,7 +42,7 @@ namespace SimulationInterfacesROS2::Utils
         {
             if (request.filters.bounds.points.size() < 3)
             {
-                return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have exactly 2 points.");
+                return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have at least 3 points.");
             }
             filter.m_bounds_shape = AZStd::make_shared<Physics::ConvexHullShapeConfiguration>();
         }

Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake

@@ -5,7 +5,10 @@ set(FILES
     Source/Clients/SimulationInterfacesROS2SystemComponent.cpp
     Source/Clients/SimulationInterfacesROS2SystemComponent.h
     Source/Interfaces/IROS2HandlerBase.h
-    Source/Services/ROS2HandlerBaseClass.h
+    Source/Actions/SimulateStepsActionServerHandler.cpp
+    Source/Actions/SimulateStepsActionServerHandler.h
+    Source/Actions/ROS2ActionBase.h
+    Source/Services/ROS2ServiceBase.h
     Source/Services/GetEntitiesServiceHandler.cpp
     Source/Services/GetEntitiesServiceHandler.h
     Source/Services/DeleteEntityServiceHandler.cpp
@@ -28,5 +31,5 @@ set(FILES
     Source/Services/GetSimulationFeaturesServiceHandler.h
     Source/Utils/RegistryUtils.cpp
     Source/Utils/RegistryUtils.h
-    Source/Utils/Utils.h    
+    Source/Utils/Utils.h
 )

Gems/SimulationInterfacesROS2/gem.json

@@ -1,28 +1,34 @@
 {
-    "gem_name": "SimulationInterfacesROS2",
-    "version": "1.0.0",
-    "display_name": "SimulationInterfacesROS2",
-    "license": "License used i.e. Apache-2.0 or MIT",
-    "license_url": "Link to the license web site i.e. https://opensource.org/licenses/Apache-2.0",
-    "origin": "The name of the originator or creator",
-    "origin_url": "The website for this Gem",
-    "type": "Code",
-    "summary": "A short description of this Gem",
-    "canonical_tags": [
-        "Gem"
-    ],
-    "user_tags": [
-        "SimulationInterfacesROS2"
-    ],
-    "platforms": [
-        ""
-    ],
-    "icon_path": "preview.png",
-    "requirements": "Notice of any requirements for this Gem i.e. This requires X other gem",
-    "documentation_url": "Link to any documentation of your Gem",
-    "dependencies": [],
-    "repo_uri": "",
-    "compatible_engines": [],
-    "engine_api_dependencies": [],
-    "restricted": "SimulationInterfacesROS2"
+  "gem_name": "SimulationInterfacesROS2",
+  "version": "1.0.0",
+  "display_name": "SimulationInterfacesROS2",
+  "license": "Apache-2.0",
+  "license_url": "https://opensource.org/licenses/Apache-2.0",
+  "origin": "RobotecAI",
+  "origin_url": "https://robotec.ai",
+  "type": "Code",
+  "summary": "This gem provides ROS 2 interface for SimulationInterfaces gem.",
+  "canonical_tags": [
+    "Gem"
+  ],
+  "user_tags": [
+    "SimulationInterfacesROS2",
+    "SimulationInterfaces",
+    "ROS2",
+    "ROS 2"
+  ],
+  "platforms": [
+    ""
+  ],
+  "icon_path": "preview.png",
+  "requirements": "ROS2, SimulationInterfaces",
+  "documentation_url": "",
+  "dependencies": [
+    "SimulationInterfaces>=1.0.0",
+    "ROS2>=3.3.0"
+  ],
+  "repo_uri": "",
+  "compatible_engines": [],
+  "engine_api_dependencies": [],
+  "restricted": "SimulationInterfacesROS2"
 }