Simulation interfaces - reset simulator service (#862)

* Simulation reset service implementation
* Adjust the ROS 2 API to support time reset.

---------

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

Gems/ROS2/Code/Include/ROS2/Clock/ITimeSource.h

@@ -8,6 +8,8 @@
 #pragma once
 
 #include <builtin_interfaces/msg/time.hpp>
+#include <AzCore/Outcome/Outcome.h>
+#include <AzCore/std/string/string.h>
 
 namespace ROS2
 {
@@ -19,6 +21,11 @@ namespace ROS2
 
         virtual ~ITimeSource() = default;
 
+        //! Sets the time source to the given time.
+        //! @param time The time to set the time source to.
+        //! @return An outcome indicating success or failure.
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time & time) = 0;
+
         //! Get time as ROS 2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const = 0;

Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h

@@ -12,6 +12,8 @@
 #include <AzCore/std/smart_ptr/unique_ptr.h>
 #include <rclcpp/publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
+#include <AzCore/Outcome/Outcome.h>
+#include <AzCore/std/string/string.h>
 
 namespace ROS2
 {
@@ -32,6 +34,8 @@ namespace ROS2
 
         builtin_interfaces::msg::Time GetROSTimestamp() const;
 
+        AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time & time) const;
+
         //! Update time in the ROS 2 ecosystem.
         //! This will publish current time to the ROS 2 `/clock` topic, if Clock is configured to do it.
         void Tick();

Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h

@@ -25,6 +25,10 @@ namespace ROS2
         //! Get ROS 2 time as ROS2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const override;
+
+
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) override;
+
     };
 
 } // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h

@@ -25,6 +25,9 @@ namespace ROS2
         virtual void Activate() override {};
         virtual void Deactivate() override {};
 
+
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) override;
+
         //! Get simulation time as ROS 2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const override;

Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h

@@ -30,6 +30,8 @@ namespace ROS2
         virtual void Activate() override;
         virtual void Deactivate() override;
 
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) override;
+
         //! Get ROS 2 time as ROS2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const override;

Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp

@@ -58,4 +58,9 @@ namespace ROS2
         msg.clock = GetROSTimestamp();
         m_clockPublisher->publish(msg);
     }
+
+    AZ::Outcome<void, AZStd::string> ROS2Clock::AdjustTime(const builtin_interfaces::msg::Time & time) const
+    {
+        return m_timeSource->AdjustTime(time);
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/ROS2TimeSource.cpp

@@ -17,4 +17,10 @@ namespace ROS2
         builtin_interfaces::msg::Time timeStamp = ros2Node->get_clock()->now();
         return timeStamp;
     }
+
+    AZ::Outcome<void, AZStd::string> ROS2TimeSource::AdjustTime(const builtin_interfaces::msg::Time& time)
+    {
+        return AZ::Failure(AZStd::string("ROS2TimeSource does not support setting a specific time."));
+    }
+
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/RealTimeSource.cpp

@@ -33,4 +33,9 @@ namespace ROS2
             return 0;
         }
     }
+
+    AZ::Outcome<void, AZStd::string> RealTimeSource::AdjustTime(const builtin_interfaces::msg::Time& time)
+    {
+        return AZ::Failure(AZStd::string("RealTimeSource does not support setting a specific time."));
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/SimulationTimeSource.cpp

@@ -83,4 +83,11 @@ namespace ROS2
         timeStamp.nanosec = static_cast<uint32_t>((m_elapsed - timeStamp.sec) * 1e9);
         return timeStamp;
     }
+
+    AZ::Outcome<void, AZStd::string> SimulationTimeSource::AdjustTime(const builtin_interfaces::msg::Time& time)
+    {
+        const double timeSec = static_cast<double>(time.sec) + static_cast<double>(time.nanosec) * 1e-9;
+        m_elapsed = timeSec;
+        return AZ::Success();
+    }
 } // namespace ROS2

Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h

@@ -80,6 +80,9 @@ namespace SimulationInterfaces
         //! @see <a href="https://github.com/ros-simulation/simulation_interfaces/blob/main/srv/DeleteEntity.srv">DeleteEntity.srv</a>
         virtual void DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) = 0;
 
+        //! Remove all previously spawned entity from the simulation.
+        virtual void DeleteAllEntities(DeletionCompletedCb completedCb) = 0;
+
         //! Get a list of spawnable entities.
         //! @see <a href="https://github.com/ros-simulation/simulation_interfaces/blob/main/srv/GetSpawnables.srv">GetSpawnables.srv</a>
         virtual AZ::Outcome<SpawnableList, FailedResult> GetSpawnables() = 0;

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

@@ -21,9 +21,14 @@ namespace SimulationInterfaces
     class SimulationManagerRequests
     {
     public:
+
         AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId);
         virtual ~SimulationManagerRequests() = default;
 
+        //! Reload level and removal of all spawned simulation entities.
+        using ReloadLevelCallback = AZStd::function<void(void)>;
+        virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0;
+
         //! Set the simulation to paused or unpaused,
         //! expect always to succeed
         virtual void SetSimulationPaused(bool paused) = 0;

Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl

@@ -226,6 +226,31 @@ namespace SimulationInterfacesCommands
         AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str());
     }
 
+    static void simulationinterfaces_ReloadLevel(const AZ::ConsoleCommandContainer& arguments)
+    {
+        SimulationManagerRequests::ReloadLevelCallback cb = []()
+        {
+            AZ_Printf("SimulationInterfacesConsole", "Reload level completed\n");
+        };
+        SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::ReloadLevel, cb);
+    }
+
+    static void simulationinterfaces_DeleteAll(const AZ::ConsoleCommandContainer& arguments)
+    {
+        DeletionCompletedCb cb = [](const AZ::Outcome<void, FailedResult>& result)
+        {
+            if (result.IsSuccess())
+            {
+                AZ_Printf("SimulationInterfacesConsole", "All spawned entities deleted\n");
+            }
+            else
+            {
+                AZ_Printf("SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().error_string.c_str());
+            }
+        };
+        SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, cb);
+    }
+
     AZ_CONSOLEFREEFUNC(
         simulationinterfaces_GetEntities, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the scene.");
 
@@ -242,4 +267,7 @@ namespace SimulationInterfacesCommands
     AZ_CONSOLEFREEFUNC(
         simulationinterfaces_GetSpawnables, AZ::ConsoleFunctorFlags::DontReplicate, "Get all spawnable entities in the scene.");
     AZ_CONSOLEFREEFUNC(simulationinterfaces_Spawn, AZ::ConsoleFunctorFlags::DontReplicate, "Spawn entity.");
+    AZ_CONSOLEFREEFUNC(simulationinterfaces_ReloadLevel, AZ::ConsoleFunctorFlags::DontReplicate, "Reload level.");
+    AZ_CONSOLEFREEFUNC(simulationinterfaces_DeleteAll, AZ::ConsoleFunctorFlags::DontReplicate, "Remove all spawned entities.");
+
 } // namespace SimulationInterfacesCommands

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

@@ -514,6 +514,35 @@ namespace SimulationInterfaces
 #endif
     }
 
+    void SimulationEntitiesManager::DeleteAllEntities(DeletionCompletedCb completedCb)
+    {
+        if (m_spawnedTickets.empty())
+        {
+            // early return for empty scene
+            completedCb(AZ::Success());
+            return;
+        }
+        for (auto m_spawnedTicket : m_spawnedTickets)
+        {
+            auto spawner = AZ::Interface<AzFramework::SpawnableEntitiesDefinition>::Get();
+            AZ_Assert(spawner, "SpawnableEntitiesDefinition is not available.");
+            // get ticket
+            auto ticket = m_spawnedTickets[m_spawnedTicket.first];
+
+            // despawn
+            AzFramework::DespawnAllEntitiesOptionalArgs optionalArgs;
+            optionalArgs.m_completionCallback = [this, completedCb](AzFramework::EntitySpawnTicket::Id ticketId)
+            {
+                m_spawnedTickets.erase(ticketId);
+                if (completedCb && m_spawnedTickets.empty())
+                {
+                    completedCb(AZ::Success());
+                }
+            };
+            spawner->DespawnAllEntities(ticket, optionalArgs);
+        }
+    }
+
     AZ::Outcome<SpawnableList, FailedResult> SimulationEntitiesManager::GetSpawnables()
     {
         AZStd::vector<Spawnable> spawnables;

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

@@ -42,6 +42,7 @@ namespace SimulationInterfaces
         AZ::Outcome<MultipleEntitiesStates, FailedResult> GetEntitiesStates(const EntityFilters& filter) override;
         AZ::Outcome<void, FailedResult> SetEntityState(const AZStd::string& name, const EntityState& state) override;
         void DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) override;
+        void DeleteAllEntities(DeletionCompletedCb completedCb) override;
         AZ::Outcome<SpawnableList, FailedResult> GetSpawnables() override;
         void SpawnEntity(
             const AZStd::string& name,

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

@@ -11,6 +11,7 @@
 
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/ConsoleBus.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h>
 #include <SimulationInterfaces/SimulationInterfacesTypeIds.h>
@@ -71,13 +72,19 @@ namespace SimulationInterfaces
 
     void SimulationManager::Activate()
     {
+        AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect();
         SimulationManagerRequestBus::Handler::BusConnect();
         SimulationFeaturesAggregatorRequestBus::Broadcast(
             &SimulationFeaturesAggregatorRequests::AddSimulationFeatures,
-            AZStd::unordered_set<SimulationFeatures>{ SimulationFeatures::SIMULATION_STATE_PAUSE,
-                                                      SimulationFeatures::STEP_SIMULATION_SINGLE,
-                                                      SimulationFeatures::STEP_SIMULATION_MULTIPLE,
-                                                      SimulationFeatures::STEP_SIMULATION_ACTION });
+            AZStd::unordered_set<SimulationFeatures>{
+                SimulationFeatures::SIMULATION_RESET,
+                SimulationFeatures::SIMULATION_RESET_TIME,
+                //SimulationFeatures::SIMULATION_RESET_STATE,
+                SimulationFeatures::SIMULATION_RESET_SPAWNED,
+                SimulationFeatures::SIMULATION_STATE_PAUSE,
+                SimulationFeatures::STEP_SIMULATION_SINGLE,
+                SimulationFeatures::STEP_SIMULATION_MULTIPLE,
+                SimulationFeatures::STEP_SIMULATION_ACTION});
     }
 
     void SimulationManager::Deactivate()
@@ -160,4 +167,34 @@ namespace SimulationInterfaces
         SetSimulationPaused(false);
     }
 
+    void SimulationManager::ReloadLevel(SimulationManagerRequests::ReloadLevelCallback completionCallback)
+    {
+        AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusConnect();
+        m_reloadLevelCallback = completionCallback;
+
+        // We need to delete all entities before reloading the level
+        DeletionCompletedCb deleteAllCompletion = [](const AZ::Outcome<void, FailedResult>& result)
+        {
+            AZ_Trace("SimulationManager", "Delete all entities completed: %s, reload level", result.IsSuccess() ? "true" : "false");
+            const char* levelName = AZ::Interface<AzFramework::ILevelSystemLifecycle>::Get()->GetCurrentLevelName();
+            AzFramework::ConsoleRequestBus::Broadcast(&AzFramework::ConsoleRequests::ExecuteConsoleCommand, "UnloadLevel");
+            AZStd::string command = AZStd::string::format("LoadLevel %s", levelName);
+            AzFramework::ConsoleRequestBus::Broadcast(&AzFramework::ConsoleRequests::ExecuteConsoleCommand, command.c_str());
+        };
+
+        // delete spawned entities
+        SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequests::DeleteAllEntities, deleteAllCompletion);
+    }
+
+    void SimulationManager::OnLoadingComplete(const char* levelName)
+    {
+        AZ_Printf("SimulationManager", "Level loading started: %s", levelName);
+        if (m_reloadLevelCallback)
+        {
+            m_reloadLevelCallback();
+            m_reloadLevelCallback = nullptr;
+        }
+        AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect();
+    }
+
 } // namespace SimulationInterfaces

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

@@ -16,11 +16,14 @@
 #include <AzFramework/Spawnable/SpawnableEntitiesInterface.h>
 #include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
 #include <SimulationInterfaces/SimulationMangerRequestBus.h>
+#include <AzFramework/API/ApplicationAPI.h>
+
 namespace SimulationInterfaces
 {
     class SimulationManager
         : public AZ::Component
         , protected SimulationManagerRequestBus::Handler
+        , protected AzFramework::LevelSystemLifecycleNotificationBus::Handler
     {
     public:
         AZ_COMPONENT_DECL(SimulationManager);
@@ -41,14 +44,21 @@ namespace SimulationInterfaces
         void Deactivate() override;
 
     protected:
+        // SimulationManagerRequestBus interface implementation
         void SetSimulationPaused(bool paused) override;
         void StepSimulation(AZ::u64 steps) override;
         bool IsSimulationPaused() const override;
         void CancelStepSimulation() override;
         bool IsSimulationStepsActive() const override;
+        void ReloadLevel(SimulationManagerRequests::ReloadLevelCallback completionCallback) override;
+
+        // LevelSystemLifecycleNotificationBus interface implementation
+        void OnLoadingComplete( const char* levelName) override;
 
+    private:
         bool m_isSimulationPaused = false;
         uint64_t m_numberOfPhysicsSteps = 0;
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent;
+        SimulationManagerRequests::ReloadLevelCallback m_reloadLevelCallback;
     };
 } // namespace SimulationInterfaces

Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp

@@ -173,6 +173,21 @@ namespace UnitTest
         DeleteEntity(entityId1);
     }
 
+    TEST_F(SimulationInterfaceTestFixture, TestEntitySpawnedDeletionEmptyScene)
+    {
+        using namespace SimulationInterfaces;
+        bool deletionWasCompleted = false;
+        DeletionCompletedCb deleteAllCompletion = [&deletionWasCompleted](const AZ::Outcome<void, FailedResult>& result)
+        {
+            deletionWasCompleted = true;
+            EXPECT_TRUE(result.IsSuccess());
+        };
+        SimulationEntityManagerRequestBus::Broadcast(
+            &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion);
+
+        TickApp(100);
+        EXPECT_TRUE(deletionWasCompleted);
+    }
 } // namespace UnitTest
 
 // required to support running integration tests with Qt and PhysX

Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp

@@ -49,6 +49,16 @@ namespace UnitTest
         AZ_Assert(assetId.IsValid(), "Failed to get asset id for %s", TestSpawnable.c_str());
     }
 
+    int getNumberOfEntities()
+    {
+        using namespace SimulationInterfaces;
+        AZ::Outcome<EntityNameList, FailedResult> enitities;
+        SimulationEntityManagerRequestBus::BroadcastResult(
+            enitities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters());
+        AZ_Assert(enitities.IsSuccess(), "Failed to get entities");
+        return enitities.GetValue().size();
+    }
+
     TEST_F(SimulationInterfaceTestFixture, SpawnAppTest)
     {
         // This is an integration test that runs the test application with the SimulationInterfaces gem enabled.
@@ -69,6 +79,7 @@ namespace UnitTest
         constexpr bool allowRename = false;
         SimulationEntityManagerRequestBus::Broadcast(
             &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, completedCb);
+
         // entities are spawned asynchronously, so we need to tick the app to let the entity be spawned
         TickApp(100);
         EXPECT_TRUE(completed);
@@ -142,12 +153,34 @@ namespace UnitTest
             &SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, deletionCompletedCb);
         TickApp(100);
 
-        // list simulation entities after deletion, expect no simulation entities
-        AZ::Outcome<EntityNameList, FailedResult> entitiesAfterDeletion;
-        SimulationEntityManagerRequestBus::BroadcastResult(
-            entitiesAfterDeletion, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters());
-        ASSERT_TRUE(entitiesAfterDeletion.IsSuccess());
-        EXPECT_EQ(entitiesAfterDeletion.GetValue().size(), 0);
+        // check if the entity was deleted
+        EXPECT_EQ(getNumberOfEntities(), 0);
+
+        // spawn 3 entities of entities and despawn all of them
+        SpawnCompletedCb cb = [&](const AZ::Outcome<AZStd::string, FailedResult>& result){};
+        SimulationEntityManagerRequestBus::Broadcast(
+            &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity1", uri, entityNamespace, initialPose, cb);
+        SimulationEntityManagerRequestBus::Broadcast(
+            &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity2", uri, entityNamespace, initialPose, cb);
+        SimulationEntityManagerRequestBus::Broadcast(
+            &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity3", uri, entityNamespace, initialPose, cb);
+        TickApp(100);
+        EXPECT_EQ(getNumberOfEntities(), 3);
+
+        // delete all entities
+        bool deletionWasCompleted = false;
+        DeletionCompletedCb deleteAllCompletion = [&deletionWasCompleted](const AZ::Outcome<void, FailedResult>& result)
+        {
+            deletionWasCompleted = true;
+            EXPECT_TRUE(result.IsSuccess());
+        };
+        SimulationEntityManagerRequestBus::Broadcast(
+            &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion);
+
+        TickApp(100);
+        EXPECT_TRUE(deletionWasCompleted);
+        EXPECT_EQ(getNumberOfEntities(), 0);
+
     }
 
 } // namespace UnitTest

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

@@ -86,6 +86,7 @@ namespace SimulationInterfacesROS2
         RegisterInterface<SetEntityStateServiceHandler>(m_availableRos2Interface, ros2Node);
         RegisterInterface<SpawnEntityServiceHandler>(m_availableRos2Interface, ros2Node);
         RegisterInterface<GetSimulationFeaturesServiceHandler>(m_availableRos2Interface, ros2Node);
+        RegisterInterface<ResetSimulationServiceHandler>(m_availableRos2Interface, ros2Node);
         RegisterInterface<SimulateStepsActionServerHandler>(m_availableRos2Interface, ros2Node);
     }
 

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

@@ -20,6 +20,7 @@
 #include "Services/ROS2ServiceBase.h"
 #include "Services/SetEntityStateServiceHandler.h"
 #include "Services/SpawnEntityServiceHandler.h"
+#include "Services/ResetSimulationServiceHandler.h"
 #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h"
 #include <AzCore/std/containers/unordered_map.h>
 #include <AzCore/std/optional.h>

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

@@ -0,0 +1,116 @@
+/*
+ * 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 "ResetSimulationServiceHandler.h"
+#include <AzCore/std/optional.h>
+#include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
+#include <SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h>
+#include <SimulationInterfaces/SimulationMangerRequestBus.h>
+#include <builtin_interfaces/msg/time.hpp>
+#include <ROS2/Clock/ROS2Clock.h>
+#include <ROS2/ROS2Bus.h>
+#include <AzCore/Component/ComponentApplicationBus.h>
+
+namespace SimulationInterfacesROS2
+{
+
+    AZStd::unordered_set<AZ::u8> ResetSimulationServiceHandler::GetProvidedFeatures()
+    {
+        return AZStd::unordered_set<AZ::u8>{ SimulationFeatures::SIMULATION_RESET,
+                                             SimulationFeatures::SIMULATION_RESET_TIME,
+                                             //SimulationFeatures::SIMULATION_RESET_STATE,
+                                             SimulationFeatures::SIMULATION_RESET_SPAWNED};
+    }
+
+    AZStd::optional<ResetSimulationServiceHandler::Response> ResetSimulationServiceHandler::HandleServiceRequest(
+        const std::shared_ptr<rmw_request_id_t> header, const Request& request)
+    {
+        if (request.scope == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_STATE)
+        {
+            Response response;
+            response.result.result = simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED;
+            response.result.error_message = "Not implemented yet";
+            return response;
+        }
+
+        if (request.scope == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_SPAWNED)
+        {
+            auto deletionCompletedCb = [this](const AZ::Outcome<void, SimulationInterfaces::FailedResult>& outcome)
+            {
+                Response response;
+                if (outcome.IsSuccess())
+                {
+                    response.result.result = simulation_interfaces::msg::Result::RESULT_OK;
+                }
+                else
+                {
+                    const auto& failedResult = outcome.GetError();
+                    response.result.result = aznumeric_cast<uint8_t>(failedResult.error_code);
+                    response.result.error_message = failedResult.error_string.c_str();
+                }
+                SendResponse(response);
+            };
+            SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast(
+                &SimulationInterfaces::SimulationEntityManagerRequests::DeleteAllEntities, deletionCompletedCb);
+            return AZStd::nullopt;
+        }
+
+        if (request.scope == Request::SCOPE_TIME)
+        {
+            auto * interface = ROS2::ROS2Interface::Get();
+            AZ_Assert(interface, "ROS2Interface is not available");
+            auto& clock = interface->GetSimulationClock();
+
+            builtin_interfaces::msg::Time time;
+            time.sec = 0;
+            time.nanosec = 0;
+            auto results = clock.AdjustTime(time);
+
+            if (results.IsSuccess())
+            {
+                Response response;
+                response.result.result = simulation_interfaces::msg::Result::RESULT_OK;
+                return response;
+            }
+            else
+            {
+                Response response;
+                response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
+                const auto & errorMessage = results.GetError();
+                response.result.error_message = std::string(errorMessage.c_str(), errorMessage.size());
+                return response;
+            }
+        }
+
+        if (request.scope == Request::SCOPE_ALL || request.scope == Request::SCOPE_DEFAULT)
+        {
+            // check if we are in GameLauncher
+            AZ::ApplicationTypeQuery appType;
+            AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationBus::Events::QueryApplicationType, appType);
+            if (appType.IsValid() && appType.IsEditor())
+            {
+                Response response;
+                response.result.result = simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED;
+                response.result.error_message = "Feature not supported in Editor.";
+                return response;
+            }
+
+            auto levelReloadCompletion = [this]()
+            {
+                Response response;
+                response.result.result = simulation_interfaces::msg::Result::RESULT_OK;
+                SendResponse(response);
+            };
+            SimulationInterfaces::SimulationManagerRequestBus::Broadcast(
+                    &SimulationInterfaces::SimulationManagerRequests::ReloadLevel, levelReloadCompletion);
+
+            return AZStd::nullopt;
+        }
+        return AZStd::nullopt;
+    }
+} // namespace SimulationInterfacesROS2

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

@@ -0,0 +1,35 @@
+/*
+ * 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 "ROS2ServiceBase.h"
+#include <AzCore/std/string/string_view.h>
+#include <rclcpp/rclcpp.hpp>
+#include <simulation_interfaces/srv/reset_simulation.hpp>
+
+namespace SimulationInterfacesROS2
+{
+    class ResetSimulationServiceHandler : public ROS2ServiceBase<simulation_interfaces::srv::ResetSimulation>
+    {
+    public:
+        AZStd::string_view GetTypeName() const override
+        {
+            return "ResetSimulation";
+        }
+
+        AZStd::string_view GetDefaultName() const override
+        {
+            return "reset_simulation";
+        }
+        AZStd::unordered_set<AZ::u8> GetProvidedFeatures() override;
+
+        AZStd::optional<Response> HandleServiceRequest(const std::shared_ptr<rmw_request_id_t> header, const Request& request) override;
+    };
+
+} // namespace SimulationInterfacesROS2

Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h

@@ -22,6 +22,7 @@ namespace UnitTest
         MOCK_METHOD1(GetEntitiesStates, AZ::Outcome<MultipleEntitiesStates, FailedResult>(const EntityFilters& filter));
         MOCK_METHOD2(SetEntityState, AZ::Outcome<void, FailedResult>(const AZStd::string& name, const EntityState& state));
         MOCK_METHOD2(DeleteEntity, void(const AZStd::string& name, DeletionCompletedCb completedCb));
+        MOCK_METHOD1(DeleteAllEntities, void(DeletionCompletedCb completedCb));
         MOCK_METHOD0(GetSpawnables, AZ::Outcome<SpawnableList, FailedResult>());
         MOCK_METHOD6(
             SpawnEntity,

Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake

@@ -27,6 +27,8 @@ set(FILES
     Source/Services/GetSpawnablesServiceHandler.h
     Source/Services/SpawnEntityServiceHandler.cpp
     Source/Services/SpawnEntityServiceHandler.h
+    Source/Services/ResetSimulationServiceHandler.cpp
+    Source/Services/ResetSimulationServiceHandler.h
     Source/Services/GetSimulationFeaturesServiceHandler.cpp
     Source/Services/GetSimulationFeaturesServiceHandler.h
     Source/Utils/RegistryUtils.cpp