Simulation Interfaces hotfixes (#879)

* Fixed wrong pose in setEntityState

Signed-off-by: Norbert Prokopiuk <[email protected]>

* Added return message in case of invalid request

Signed-off-by: Norbert Prokopiuk <[email protected]>

---------

Signed-off-by: Norbert Prokopiuk <[email protected]>

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

@@ -160,7 +160,7 @@ namespace SimulationInterfaces
 
                 // cache the initial state
                 EntityState initialState{};
-                initialState.m_pose = entity->GetTransform()->GetLocalTM();
+                initialState.m_pose = entity->GetTransform()->GetWorldTM();
                 if (rigidBody)
                 {
                     initialState.m_twist_linear = rigidBody->GetLinearVelocity();
@@ -221,10 +221,10 @@ namespace SimulationInterfaces
 
         SimulationFeaturesAggregatorRequestBus::Broadcast(
             &SimulationFeaturesAggregatorRequests::AddSimulationFeatures,
-            AZStd::unordered_set<SimulationFeatures>{//simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS,
+            AZStd::unordered_set<SimulationFeatures>{ // simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS,
                                                       simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX,
-                                                      //simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX,
-                                                      //simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES,
+                                                      // simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX,
+                                                      // simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES,
                                                       simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING,
                                                       simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING,
                                                       simulation_interfaces::msg::SimulatorFeatures::DELETING,
@@ -444,6 +444,8 @@ namespace SimulationInterfaces
             return;
         }
         const AZ::EntityId entityId = entityAndDescendants.front();
+        AZ::EntityId parentEntityId = AZ::EntityId{ AZ::EntityId::InvalidEntityId };
+        AZ::TransformBus::EventResult(parentEntityId, entityId, &AZ::TransformBus::Events::GetParentId);
         if (state.m_pose.IsOrthogonal())
         {
             // disable simulation for all entities
@@ -451,8 +453,17 @@ namespace SimulationInterfaces
             {
                 Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics);
             }
-
-            AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose);
+            if (parentEntityId.IsValid())
+            {
+                AZ::Transform parentTransform;
+                AZ::TransformBus::EventResult(parentTransform, parentEntityId, &AZ::TransformBus::Events::GetWorldTM);
+                auto transformToSet = parentTransform.GetInverse() * state.m_pose;
+                AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, transformToSet);
+            }
+            else
+            {
+                AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose);
+            }
 
             for (const auto& descendant : entityAndDescendants)
             {
@@ -679,20 +690,13 @@ namespace SimulationInterfaces
                 AZStd::string entityName = AZStd::string::format("%s_%s", name.c_str(), entity->GetName().c_str());
                 entity->SetName(entityName);
             }
+            const AZ::Entity* root = *view.begin();
 
-            // get the first entity
-            if (view.size()>1)
+            auto* transformInterface = root->FindComponent<AzFramework::TransformComponent>();
+            if (transformInterface)
             {
-                const AZ::Entity* firstEntity = view[1];
-                AZ_Assert(firstEntity, "First entity is not available");
-                auto* transformInterface = firstEntity->FindComponent<AzFramework::TransformComponent>();
-                if (transformInterface)
-                {
-                    transformInterface->SetWorldTM(initialPose);
-                }
+                transformInterface->SetWorldTM(initialPose);
             }
-
-
         };
         optionalArgs.m_completionCallback =
             [this, uri](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view)

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

@@ -7,14 +7,15 @@
  */
 
 #include "ResetSimulationServiceHandler.h"
+#include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/std/optional.h>
+#include <ROS2/Clock/ROS2Clock.h>
+#include <ROS2/ROS2Bus.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>
+#include <simulation_interfaces/msg/detail/result__struct.hpp>
 
 namespace ROS2SimulationInterfaces
 {
@@ -24,7 +25,7 @@ namespace ROS2SimulationInterfaces
         return AZStd::unordered_set<AZ::u8>{ SimulationFeatures::SIMULATION_RESET,
                                              SimulationFeatures::SIMULATION_RESET_TIME,
                                              SimulationFeatures::SIMULATION_RESET_STATE,
-                                             SimulationFeatures::SIMULATION_RESET_SPAWNED};
+                                             SimulationFeatures::SIMULATION_RESET_SPAWNED };
     }
 
     AZStd::optional<ResetSimulationServiceHandler::Response> ResetSimulationServiceHandler::HandleServiceRequest(
@@ -63,7 +64,7 @@ namespace ROS2SimulationInterfaces
 
         if (request.scope == Request::SCOPE_TIME)
         {
-            auto * interface = ROS2::ROS2Interface::Get();
+            auto* interface = ROS2::ROS2Interface::Get();
             AZ_Assert(interface, "ROS2Interface is not available");
             auto& clock = interface->GetSimulationClock();
 
@@ -82,7 +83,7 @@ namespace ROS2SimulationInterfaces
             {
                 Response response;
                 response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
-                const auto & errorMessage = results.GetError();
+                const auto& errorMessage = results.GetError();
                 response.result.error_message = std::string(errorMessage.c_str(), errorMessage.size());
                 return response;
             }
@@ -108,10 +109,15 @@ namespace ROS2SimulationInterfaces
                 SendResponse(response);
             };
             SimulationInterfaces::SimulationManagerRequestBus::Broadcast(
-                    &SimulationInterfaces::SimulationManagerRequests::ReloadLevel, levelReloadCompletion);
+                &SimulationInterfaces::SimulationManagerRequests::ReloadLevel, levelReloadCompletion);
 
             return AZStd::nullopt;
         }
-        return AZStd::nullopt;
+
+        // no case matched, return response that request was invalid
+        Response invalidResponse;
+        invalidResponse.result.result = simulation_interfaces::msg::Result::RESULT_NOT_FOUND;
+        invalidResponse.result.error_message = "Passed unknown scope";
+        return invalidResponse;
     }
 } // namespace ROS2SimulationInterfaces