diff --git a/Gems/SimulationInterfaces/.gitignore b/Gems/SimulationInterfaces/.gitignore deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h index 865ec4ded4..db4a75ac39 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h @@ -8,7 +8,7 @@ #pragma once -#include +#include "ROS2SimulationInterfacesTypeIds.h" #include #include @@ -16,6 +16,7 @@ namespace ROS2SimulationInterfaces { + using SimulationFeatureType = uint8_t; class ROS2SimulationInterfacesRequests { public: @@ -23,9 +24,9 @@ namespace ROS2SimulationInterfaces virtual ~ROS2SimulationInterfacesRequests() = default; //! Returns set of simulation features available in ROS2SimulationInterfaces Gem - //! SimulationFeatures follows definition available at: + //! SimulationFeatureType follows definition available at: //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg - virtual AZStd::unordered_set GetSimulationFeatures() = 0; + virtual AZStd::unordered_set GetSimulationFeatures() = 0; }; class ROS2SimulationInterfacesRequestBusTraits : public AZ::EBusTraits diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h index d7fdeca067..6a9de009db 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h @@ -13,18 +13,18 @@ namespace SimulationInterfaces { //! Result codes to be used in the Result message //! @see Result.msg - using ErrorCodeValue = uint8_t; + using ErrorCodeType = uint8_t; //! A message type to represent the result of a failed operation struct FailedResult { FailedResult() = default; - FailedResult(ErrorCodeValue error_code, const AZStd::string& error_string) - : error_code(error_code) - , error_string(error_string) + FailedResult(ErrorCodeType errorCode, const AZStd::string& errorString) + : m_errorCode(errorCode) + , m_errorString(errorString) { } - ErrorCodeValue error_code; - AZStd::string error_string; + ErrorCodeType m_errorCode; + AZStd::string m_errorString; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index c930b70a47..bf2bca2feb 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -8,7 +8,7 @@ #pragma once -#include +#include "SimulationInterfacesTypeIds.h" #include "Result.h" #include "TagFilter.h" @@ -19,30 +19,33 @@ namespace SimulationInterfaces { - //! # A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. + //! A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. //! @see EntityFilters.msg struct EntityFilters { - AZStd::string m_filter; //! A posix regular expression to match against entity names - TagFilter m_tags_filter; //! A filter to match against entity tags + //! A posix regular expression to match against entity names, + //! The regular expression syntax is POSIX Extended, + //! @see POSIX_Extended definitions + AZStd::string m_nameFilter; + TagFilter m_tagsFilter; //! A filter to match against entity tags AZStd::shared_ptr - m_bounds_shape; //! A shape to use for filtering entities, null means no bounds filtering - AZ::Transform m_bounds_pose{ AZ::Transform::CreateIdentity() }; + m_boundsShape; //! A shape to use for filtering entities, null means no bounds filtering + AZ::Transform m_boundsPose{ AZ::Transform::CreateIdentity() }; }; //! @see EntityState.msg struct EntityState { AZ::Transform m_pose; //! The pose of the entity - AZ::Vector3 m_twist_linear; //! The linear velocity of the entity (in the entity frame) - AZ::Vector3 m_twist_angular; //! The angular velocity of the entity (in the entity frame) + AZ::Vector3 m_twistLinear; //! The linear velocity of the entity (in the entity frame) + AZ::Vector3 m_twistAngular; //! The angular velocity of the entity (in the entity frame) }; struct Spawnable { AZStd::string m_uri; AZStd::string m_description; - AZStd::string m_bounds_sphere; + AZStd::string m_boundsSphere; }; using EntityNameList = AZStd::vector; @@ -50,6 +53,7 @@ namespace SimulationInterfaces using SpawnableList = AZStd::vector; using DeletionCompletedCb = AZStd::function&)>; using SpawnCompletedCb = AZStd::function&)>; + class SimulationEntityManagerRequests { public: diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h index b5b325d842..805a03b228 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -8,7 +8,7 @@ #pragma once -#include +#include "SimulationInterfacesTypeIds.h" #include #include @@ -16,7 +16,7 @@ namespace SimulationInterfaces { - using SimulationFeatures = uint8_t; + using SimulationFeatureType = uint8_t; class SimulationFeaturesAggregatorRequests { @@ -25,15 +25,15 @@ namespace SimulationInterfaces virtual ~SimulationFeaturesAggregatorRequests() = default; //! Registers simulation features defined by caller - virtual void AddSimulationFeatures(const AZStd::unordered_set& features) = 0; + virtual void AddSimulationFeatures(const AZStd::unordered_set& features) = 0; //! Returns features available in the simulator, list follows definitions at //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg - virtual const AZStd::unordered_set GetSimulationFeatures() const = 0; + virtual AZStd::unordered_set GetSimulationFeatures() = 0; //! Method checks if feature with given id is available in the simulation //! Method is extension to standard defined in simulation_interfaces - virtual bool HasFeature(SimulationFeatures feature) const = 0; + virtual bool HasFeature(SimulationFeatureType feature) = 0; }; class SimulationFeaturesAggregatorRequestBusTraits : public AZ::EBusTraits diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 63bb03822a..17f45935b8 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -8,25 +8,27 @@ #pragma once -#include +#include "SimulationInterfacesTypeIds.h" +#include "Result.h" #include #include #include #include -#include namespace SimulationInterfaces { using SimulationState = uint8_t; + class SimulationManagerRequests { public: AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); virtual ~SimulationManagerRequests() = default; - //! Reload level and removal of all spawned simulation entities. using ReloadLevelCallback = AZStd::function; + + //! Reload level and removal of all spawned simulation entities. virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0; //! Set the simulation to paused or unpaused, diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h index cdd0660a05..1a1bb0e0bf 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h @@ -15,14 +15,10 @@ namespace SimulationInterfaces { //! Structure to design a filter for tags //! @see TagsFilter.msg + using TagFilterMode = uint8_t; struct TagFilter { - enum class TagFilterMode - { - FILTER_MODE_ANY, - FILTER_MODE_ALL - }; AZStd::unordered_set m_tags; TagFilterMode m_mode; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h index 6fcacb10bf..d36648f259 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h @@ -11,7 +11,6 @@ #include #include #include -#include #include #include diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp index d50ffb57e1..dfbe95d037 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp @@ -28,4 +28,4 @@ namespace SimulationInterfaces::Utils } return {}; } -} // namespace SimulationInterfaces::Utils \ No newline at end of file +} // namespace SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h index 0d0a96a802..2ca5f2c4ca 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h @@ -17,4 +17,4 @@ namespace SimulationInterfaces::Utils AZStd::string RelPathToUri(AZStd::string_view relPath); AZStd::string UriToRelPath(AZStd::string_view relPath); -} // namespace SimulationInterfaces::Utils \ No newline at end of file +} // namespace SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl index 620c808163..38dd7d7984 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl @@ -20,10 +20,11 @@ namespace SimulationInterfacesCommands static void simulationinterfaces_GetEntities(const AZ::ConsoleCommandContainer& arguments) { AZ::Outcome entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); if (!entities.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str()); return; } @@ -67,13 +68,13 @@ namespace SimulationInterfacesCommands AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntities in radius %f \n", sphereShape); AZ_Printf("SimulationInterfacesConsole", "position %f %f %f \n", position.GetX(), position.GetY(), position.GetZ()); EntityFilters filter; - filter.m_bounds_shape = AZStd::make_shared(sphereShape); + filter.m_boundsShape = AZStd::make_shared(sphereShape); AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); if (!entities.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str()); return; } @@ -93,33 +94,23 @@ namespace SimulationInterfacesCommands const AZStd::string entityName = arguments[0]; AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState %s\n", entityName.c_str()); AZ::Outcome entityStateResult; - SimulationEntityManagerRequestBus::BroadcastResult(entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + SimulationEntityManagerRequestBus::BroadcastResult( + entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); if (!entityStateResult.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().error_string.c_str()); + AZ_Printf( + "SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().m_errorString.c_str()); return; } - const auto &entityState = entityStateResult.GetValue(); + const auto& entityState = entityStateResult.GetValue(); AZ_Printf("SimulationInterfacesConsole", "Entity %s\n", entityName.c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Pose %s\n", - AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Rotation %s \n", - AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Pose %s\n", AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Rotation %s \n", AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str()); const AZ::Vector3 euler = entityState.m_pose.GetRotation().GetEulerDegrees(); AZ_Printf("SimulationInterfacesConsole", "Rotation (euler) %s\n", AZ::Vector3ToString(euler).c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Twist Linear %s\n", - AZ::Vector3ToString(entityState.m_twist_linear).c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Twist Angular %s\n", - AZ::Vector3ToString(entityState.m_twist_angular).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Twist Linear %s\n", AZ::Vector3ToString(entityState.m_twistLinear).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Twist Angular %s\n", AZ::Vector3ToString(entityState.m_twistAngular).c_str()); } static void simulationinterfaces_SetStateXYZ(const AZ::ConsoleCommandContainer& arguments) @@ -143,11 +134,10 @@ namespace SimulationInterfacesCommands if (!result.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().m_errorString.c_str()); return; } AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str()); - } static void simulationinterfaces_DeleteEntity(const AZ::ConsoleCommandContainer& arguments) @@ -166,11 +156,10 @@ namespace SimulationInterfacesCommands } else { - AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().m_errorString.c_str()); } }; SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, cb); - } static void simulationinterfaces_GetSpawnables(const AZ::ConsoleCommandContainer& arguments) @@ -180,7 +169,7 @@ namespace SimulationInterfacesCommands SimulationEntityManagerRequestBus::BroadcastResult(spawnables, &SimulationEntityManagerRequestBus::Events::GetSpawnables); if (!spawnables.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().m_errorString.c_str()); return; } for (const auto& spawnable : spawnables.GetValue()) @@ -205,24 +194,23 @@ namespace SimulationInterfacesCommands AZ::Transform initialPose = AZ::Transform::CreateIdentity(); if (arguments.size() > 5) { - initialPose.SetTranslation( - AZ::Vector3( - AZStd::stof(AZStd::string(arguments[3])), - AZStd::stof(AZStd::string(arguments[4])), - AZStd::stof(AZStd::string(arguments[5])))); + initialPose.SetTranslation(AZ::Vector3( + AZStd::stof(AZStd::string(arguments[3])), + AZStd::stof(AZStd::string(arguments[4])), + AZStd::stof(AZStd::string(arguments[5])))); } SpawnCompletedCb completedCb = [](const AZ::Outcome& result) { if (!result.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().m_errorString.c_str()); return; } AZ_Printf("SimulationInterfacesConsole", "Entity spawned and registered : %s\n", result.GetValue().c_str()); - }; constexpr bool allowRename = true; - SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb); + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb); AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); } @@ -245,7 +233,8 @@ namespace SimulationInterfacesCommands } else { - AZ_Printf("SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().error_string.c_str()); + AZ_Printf( + "SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().m_errorString.c_str()); } }; SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, cb); @@ -258,7 +247,6 @@ namespace SimulationInterfacesCommands AZ_CONSOLEFREEFUNC(simulationinterfaces_Resume, AZ::ConsoleFunctorFlags::DontReplicate, "Resume simulation."); AZ_CONSOLEFREEFUNC(simulationinterfaces_Step, AZ::ConsoleFunctorFlags::DontReplicate, "Step simulation."); - AZ_CONSOLEFREEFUNC( simulationinterfaces_GetEntitiesSphere, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the radius."); AZ_CONSOLEFREEFUNC(simulationinterfaces_GetEntityState, AZ::ConsoleFunctorFlags::DontReplicate, "Get state of the entity."); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 3cee2904b7..a6de9dfc2d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -28,7 +29,6 @@ #include #include #include -#include #include #include @@ -37,22 +37,34 @@ namespace SimulationInterfaces { - void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) + namespace { - if (!state.m_twist_angular.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) { - // get transform - AZ::Vector3 angularVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_angular); - rigidBody->SetAngularVelocity(angularVelWorld); + if (!state.m_twistAngular.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + { + // get transform + AZ::Vector3 angularVelWorld = rigidBody->GetTransform().TransformVector(state.m_twistAngular); + rigidBody->SetAngularVelocity(angularVelWorld); + } + + if (!state.m_twistLinear.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + { + // get transform + AZ::Vector3 linearVelWorld = rigidBody->GetTransform().TransformVector(state.m_twistLinear); + rigidBody->SetLinearVelocity(linearVelWorld); + } } - if (!state.m_twist_linear.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + AzPhysics::Scene* GetSceneHelper(AzPhysics::SceneHandle sceneHandle) { - // get transform - AZ::Vector3 linearVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_linear); - rigidBody->SetLinearVelocity(linearVelWorld); + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "Physics system is not available."); + AzPhysics::Scene* scene = physicsSystem->GetScene(sceneHandle); + return scene; } - } + + } // namespace AZ_COMPONENT_IMPL(SimulationEntitiesManager, "SimulationEntitiesManager", SimulationEntitiesManagerTypeId); @@ -106,15 +118,6 @@ namespace SimulationInterfaces { } - AzPhysics::Scene* GetSceneHelper(AzPhysics::SceneHandle sceneHandle) - { - AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); - AZ_Assert(physicsSystem, "Physics system is not available."); - AzPhysics::Scene* scene = physicsSystem->GetScene(sceneHandle); - return scene; - } - - bool SimulationEntitiesManager::RegisterNewSimulatedBody(AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) { auto* scene = GetSceneHelper(sceneHandle); @@ -158,8 +161,7 @@ namespace SimulationInterfaces auto spawnDataIt = m_spawnCompletedCallbacks.find(ticketId); const bool wasSpawned = spawnDataIt != m_spawnCompletedCallbacks.end(); - const AZStd::string proposedName = - wasSpawned ? spawnDataIt->second.m_userProposedName : entity->GetName(); + const AZStd::string proposedName = wasSpawned ? spawnDataIt->second.m_userProposedName : entity->GetName(); // register entity const AZStd::string registeredName = this->AddSimulatedEntity(entityId, proposedName); @@ -176,8 +178,8 @@ namespace SimulationInterfaces initialState.m_pose = entity->GetTransform()->GetWorldTM(); if (rigidBody) { - initialState.m_twist_linear = rigidBody->GetLinearVelocity(); - initialState.m_twist_angular = rigidBody->GetAngularVelocity(); + initialState.m_twistLinear = rigidBody->GetLinearVelocity(); + initialState.m_twistAngular = rigidBody->GetAngularVelocity(); } m_entityIdToInitialState[entityId] = initialState; AZ_Info("SimulationInterfaces", "Registered entity %s\n", registeredName.c_str()); @@ -224,7 +226,10 @@ namespace SimulationInterfaces m_sceneAddedHandler = AzPhysics::SystemEvents::OnSceneAddedEvent::Handler( [this](AzPhysics::SceneHandle sceneHandle) { - AZ_Warning("SimulationInterfaces", m_physicsScenesHandle == AzPhysics::InvalidSceneHandle, "Hmm, we already have a scene"); + AZ_Warning( + "SimulationInterfaces", + m_physicsScenesHandle == AzPhysics::InvalidSceneHandle, + "SimulationInterfaces already gathered physics scene"); auto* scene = GetSceneHelper(sceneHandle); AZ_Assert(scene, "Scene is not available."); if (scene == nullptr) @@ -259,15 +264,15 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ // 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_STATE_GETTING, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, - simulation_interfaces::msg::SimulatorFeatures::DELETING, - simulation_interfaces::msg::SimulatorFeatures::SPAWNABLES, - simulation_interfaces::msg::SimulatorFeatures::SPAWNING }); + AZStd::unordered_set{ // 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_STATE_GETTING, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, + simulation_interfaces::msg::SimulatorFeatures::DELETING, + simulation_interfaces::msg::SimulatorFeatures::SPAWNABLES, + simulation_interfaces::msg::SimulatorFeatures::SPAWNING }); AZ::TickBus::Handler::BusConnect(); } @@ -275,28 +280,18 @@ namespace SimulationInterfaces { AZ::TickBus::Handler::BusDisconnect(); SimulationEntityManagerRequestBus::Handler::BusDisconnect(); - if (m_simulationBodyAddedHandler.IsConnected()) - { - m_simulationBodyAddedHandler.Disconnect(); - } - if (m_simulationBodyRemovedHandler.IsConnected()) - { - m_simulationBodyRemovedHandler.Disconnect(); - } + + m_simulationBodyAddedHandler.Disconnect(); + m_simulationBodyRemovedHandler.Disconnect(); + m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; - if (m_sceneAddedHandler.IsConnected()) - { - m_sceneAddedHandler.Disconnect(); - } - if (m_sceneAddedHandler.IsConnected()) - { - m_sceneAddedHandler.Disconnect(); - } + + m_sceneAddedHandler.Disconnect(); + m_sceneAddedHandler.Disconnect(); } AZStd::string SimulationEntitiesManager::AddSimulatedEntity(AZ::EntityId entityId, const AZStd::string& userProposedName) { - if (!entityId.IsValid()) { return ""; @@ -332,15 +327,15 @@ namespace SimulationInterfaces AZ::Outcome SimulationEntitiesManager::GetEntities(const EntityFilters& filter) { - if (!filter.m_tags_filter.m_tags.empty()) + if (!filter.m_tagsFilter.m_tags.empty()) { AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); return AZ::Failure( FailedResult(simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); } - const bool reFilter = !filter.m_filter.empty(); - const bool shapeCastFilter = filter.m_bounds_shape != nullptr; + const bool reFilter = !filter.m_nameFilter.empty(); + const bool shapeCastFilter = filter.m_boundsShape != nullptr; AZStd::vector entities; if (!shapeCastFilter) @@ -368,8 +363,8 @@ namespace SimulationInterfaces } AzPhysics::OverlapRequest request; - request.m_shapeConfiguration = filter.m_bounds_shape; - request.m_pose = filter.m_bounds_pose; + request.m_shapeConfiguration = filter.m_boundsShape; + request.m_pose = filter.m_boundsPose; request.m_maxResults = AZStd::numeric_limits::max(); AzPhysics::SceneQueryHits result = sceneInterface->QueryScene(m_physicsScenesHandle, &request); @@ -387,7 +382,7 @@ namespace SimulationInterfaces { const AZStd::vector prefilteredEntities = AZStd::move(entities); entities.clear(); - const AZStd::regex regex(filter.m_filter); + const AZStd::regex regex(filter.m_nameFilter); if (!regex.Valid()) { AZ_Warning("SimulationInterfaces", false, "Invalid regex filter"); @@ -426,14 +421,14 @@ namespace SimulationInterfaces // transform linear and angular velocities to entity frame const AZ::Transform entityTransformInv = entityState.m_pose.GetInverse(); - entityState.m_twist_linear = entityTransformInv.TransformVector(linearVelocity); - entityState.m_twist_angular = entityTransformInv.TransformVector(angularVelocity); + entityState.m_twistLinear = entityTransformInv.TransformVector(linearVelocity); + entityState.m_twistAngular = entityTransformInv.TransformVector(angularVelocity); return AZ::Success(entityState); } AZ::Outcome SimulationEntitiesManager::GetEntitiesStates(const EntityFilters& filter) { - if (!filter.m_tags_filter.m_tags.empty()) + if (!filter.m_tagsFilter.m_tags.empty()) { AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); return AZ::Failure( @@ -514,7 +509,7 @@ namespace SimulationInterfaces } } - if (!state.m_twist_linear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twist_angular.IsZero(AZ::Constants::FloatEpsilon)) + if (!state.m_twistLinear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twistAngular.IsZero(AZ::Constants::FloatEpsilon)) { // get rigid body AzPhysics::RigidBody* rigidBody = nullptr; @@ -571,23 +566,6 @@ namespace SimulationInterfaces const auto msg = AZStd::string::format("Entity %s was not spawned by this component, wont delete it", name.c_str()); completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, msg))); } -#ifdef POTENTIALY_UNSAFE - if (findIt != m_simulatedEntityToEntityIdMap.end()) - { - const AZ::EntityId entityId = findIt->second; - AZ_Assert(entityId.IsValid(), "EntityId is not valid"); - // get all descendants - AZStd::vector entityAndDescendants; - AZ::TransformBus::EventResult(entityAndDescendants, entityId, &AZ::TransformBus::Events::GetEntityAndAllDescendants); - for (const auto& descendant : entityAndDescendants) - { - // I am not sure if this is the safe way to delete an entity - AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationRequests::DeleteEntity, descendant); - } - - return true; - } -#endif } void SimulationEntitiesManager::DeleteAllEntities(DeletionCompletedCb completedCb) @@ -790,7 +768,7 @@ namespace SimulationInterfaces } // check if name is still not unique, if not, add EntityId to name - if(m_simulatedEntityToEntityIdMap.contains(newName)) + if (m_simulatedEntityToEntityIdMap.contains(newName)) { newName = AZStd::string::format("%s_%s", newName.c_str(), entityId.ToString().c_str()); } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index 244033f2dd..7322467ae8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -35,8 +35,12 @@ namespace SimulationInterfaces SimulationEntitiesManager(); ~SimulationEntitiesManager(); + // AZ::Component interface implementation + void Init() override; + void Activate() override; + void Deactivate() override; - protected: + private: // SimulationEntityManagerRequestBus interface implementation AZ::Outcome GetEntities(const EntityFilters& filter) override; AZ::Outcome GetEntityState(const AZStd::string& name) override; @@ -54,16 +58,9 @@ namespace SimulationInterfaces SpawnCompletedCb completedCb) override; void ResetAllEntitiesToInitialState() override; - // AZ::Component interface implementation - void Init() override; - void Activate() override; - void Deactivate() override; - // AZ::TickBus::Handler interface implementation void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; - private: - //! Registers a new simulated body to the simulation interface. //! Note that the body handle will be registered under unique name //! Note that body need to be configured to be registered @@ -73,7 +70,8 @@ namespace SimulationInterfaces //! Registers a new simulated body to the simulation interface. //! Returns the list of handles that were not registered - AZStd::vector> RegisterNewSimulatedBodies(const AZStd::vector>& handles); + AZStd::vector> RegisterNewSimulatedBodies( + const AZStd::vector>& handles); //! Registers simulated entity to entity id mapping. //! Note that the entityId will be registered under unique name. @@ -91,7 +89,6 @@ namespace SimulationInterfaces //! Set the state of the entity and their descendants. void SetEntitiesState(const AZStd::vector& entityAndDescendants, const EntityState& state); - AzPhysics::SceneEvents::OnSimulationBodyAdded::Handler m_simulationBodyAddedHandler; AzPhysics::SceneEvents::OnSimulationBodyRemoved::Handler m_simulationBodyRemovedHandler; @@ -99,7 +96,8 @@ namespace SimulationInterfaces AzPhysics::SystemEvents::OnSceneRemovedEvent::Handler m_sceneRemovedHandler; AzPhysics::SceneHandle m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; - AZStd::vector> m_unconfiguredScenesHandles; //! Set of yet-invalid scenes handles, that are waiting for configuration + AZStd::vector> + m_unconfiguredScenesHandles; //! Set of yet-invalid scenes handles, that are waiting for configuration AZStd::unordered_map m_simulatedEntityToEntityIdMap; AZStd::unordered_map m_entityIdToSimulatedEntityMap; AZStd::unordered_map m_entityIdToInitialState; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp index f73760c13e..699779c1ac 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp @@ -74,17 +74,17 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Handler::BusDisconnect(); } - void SimulationFeaturesAggregator::AddSimulationFeatures(const AZStd::unordered_set& features) + void SimulationFeaturesAggregator::AddSimulationFeatures(const AZStd::unordered_set& features) { m_registeredFeatures.insert(features.begin(), features.end()); } - - const AZStd::unordered_set SimulationFeaturesAggregator::GetSimulationFeatures() const + + AZStd::unordered_set SimulationFeaturesAggregator::GetSimulationFeatures() { return m_registeredFeatures; } - bool SimulationFeaturesAggregator::HasFeature(SimulationFeatures feature) const + bool SimulationFeaturesAggregator::HasFeature(SimulationFeatureType feature) { return m_registeredFeatures.contains(feature); } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h index 8fa8abe76c..9f445a1d48 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h @@ -42,13 +42,12 @@ namespace SimulationInterfaces void Activate() override; void Deactivate() override; - protected: + private: // SimulationFeaturesAggregatorRequestBus overrides - void AddSimulationFeatures(const AZStd::unordered_set& features) override; - const AZStd::unordered_set GetSimulationFeatures() const override; - bool HasFeature(SimulationFeatures feature) const override; + void AddSimulationFeatures(const AZStd::unordered_set& features) override; + AZStd::unordered_set GetSimulationFeatures() override; + bool HasFeature(SimulationFeatureType feature) override; - private: - AZStd::unordered_set m_registeredFeatures; + AZStd::unordered_set m_registeredFeatures; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index fa3adb5750..2c941584c5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -105,16 +105,16 @@ namespace SimulationInterfaces SimulationManagerRequestBus::Handler::BusConnect(); SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_TIME, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_STATE, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_SPAWNED, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_PAUSE, - simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_SINGLE, - simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_MULTIPLE, - simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_ACTION, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_SETTING, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_GETTING }); + AZStd::unordered_set{ simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_TIME, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_STATE, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_SPAWNED, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_PAUSE, + simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_SINGLE, + simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_MULTIPLE, + simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_ACTION, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_SETTING, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_GETTING }); AZ::SystemTickBus::QueueFunction( [this]() { diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index bc8f85b5af..7db04c7704 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -46,7 +46,7 @@ namespace SimulationInterfaces void Activate() override; void Deactivate() override; - protected: + private: // SimulationManagerRequestBus interface implementation void SetSimulationPaused(bool paused) override; void StepSimulation(AZ::u64 steps) override; @@ -60,7 +60,6 @@ namespace SimulationInterfaces // LevelSystemLifecycleNotificationBus interface implementation void OnLoadingComplete(const char* levelName) override; - private: bool m_isSimulationPaused = false; uint64_t m_numberOfPhysicsSteps = 0; AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; @@ -70,7 +69,6 @@ namespace SimulationInterfaces }; // default simulation state based on standard void InitializeSimulationState(); - private: bool IsTransitionForbidden(SimulationState requestedState); // forbidden transition between state, first is current state, second is desire state const AZStd::array, 4> m_forbiddenStatesTransitions{ { diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp index 22812e11ca..c5b31a09c4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -36,8 +36,8 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); }); diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp index 66f2d41539..ead483edc7 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -43,8 +43,8 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index 9c3842ea24..77cee8486a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -45,40 +45,28 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } const auto& multipleEntitiesStates = outcome.GetValue(); - std::vector stdEntities; - std::vector stdEntityStates; - AZStd::transform( - multipleEntitiesStates.begin(), - multipleEntitiesStates.end(), - std::back_inserter(stdEntities), - [](const auto& pair) - { - return pair.first.c_str(); - }); - AZStd::transform( - multipleEntitiesStates.begin(), - multipleEntitiesStates.end(), - std::back_inserter(stdEntityStates), - [](const auto& pair) - { - const SimulationInterfaces::EntityState& entityState = pair.second; - 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; + response.entities.reserve(multipleEntitiesStates.size()); + response.states.reserve(multipleEntitiesStates.size()); + for (auto& [entityName, entityState] : multipleEntitiesStates) + { + // entity name + response.entities.push_back(entityName.c_str()); + // entity state + simulation_interfaces::msg::EntityState simulationInterfacesEntityState; + simulationInterfacesEntityState.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + simulationInterfacesEntityState.header.frame_id = ""; + simulationInterfacesEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); + simulationInterfacesEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistLinear); + simulationInterfacesEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistAngular); + response.states.push_back(simulationInterfacesEntityState); + } return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp index 00b50f013f..0bf0588d8a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -33,18 +33,18 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } const auto& entityState = outcome.GetValue(); simulation_interfaces::msg::EntityState entityStateMsg; entityStateMsg.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); - entityStateMsg.header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); + entityStateMsg.header.frame_id = ""; entityStateMsg.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); - entityStateMsg.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); - entityStateMsg.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); + entityStateMsg.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistLinear); + entityStateMsg.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistAngular); response.result.result = simulation_interfaces::msg::Result::RESULT_OK; response.state = entityStateMsg; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 5a4aea23d6..658150ebe5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -10,37 +10,36 @@ #include #include #include -#include namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() + AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() { // standard doesn't define specific feature id for this service - return AZStd::unordered_set{}; + return AZStd::unordered_set{}; } AZStd::optional GetSimulationFeaturesServiceHandler::HandleServiceRequest( const std::shared_ptr header, const Request& request) { // call bus to get simulation features in ROS2SimulationInterfaces Gem side - AZStd::unordered_set ros2Interfaces; + AZStd::unordered_set ros2Interfaces; ROS2SimulationInterfacesRequestBus::BroadcastResult(ros2Interfaces, &ROS2SimulationInterfacesRequests::GetSimulationFeatures); // call bus to get simulation features on SimulationInterfaces Gem side - AZStd::unordered_set o3deInterfaces; + AZStd::unordered_set o3deInterfaces; SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::BroadcastResult( o3deInterfaces, &SimulationInterfaces::SimulationFeaturesAggregatorRequests::GetSimulationFeatures); // create common features and return it; // common features are logical AND between two sets - AZStd::unordered_set commonFeatures; + AZStd::unordered_set commonFeatures; commonFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); commonFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end()); Response response; for (auto id : commonFeatures) { - if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatures(id))) + if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatureType(id))) { response.features.features.emplace_back(id); } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h index 568ade2e66..8e993943b4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -10,6 +10,7 @@ #include "ROS2ServiceBase.h" #include +#include #include namespace ROS2SimulationInterfaces @@ -27,7 +28,7 @@ namespace ROS2SimulationInterfaces { return "get_simulation_features"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h index 2ca83acdbc..a7cb5e4ebc 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h @@ -10,7 +10,6 @@ #include "Services/ROS2ServiceBase.h" #include -#include #include namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp index c2fa782ba3..2baba4a642 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -28,17 +28,16 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } const auto& spawnableList = outcome.GetValue(); - std::vector simSpawnables; AZStd::transform( spawnableList.begin(), spawnableList.end(), - AZStd::back_inserter(simSpawnables), + AZStd::back_inserter(response.spawnables), [](const SimulationInterfaces::Spawnable& spawnable) { simulation_interfaces::msg::Spawnable simSpawnable; @@ -47,7 +46,6 @@ namespace ROS2SimulationInterfaces return simSpawnable; }); response.result.result = simulation_interfaces::msg::Result::RESULT_OK; - response.spawnables = simSpawnables; return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h index 6e095a2958..b53f46c748 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h @@ -12,7 +12,6 @@ #include #include #include -#include #include #include diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index 5a2ff4e0c9..e778126a97 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -52,8 +52,8 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); }; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h index 1bb965fd25..b98995c4bd 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h @@ -10,7 +10,6 @@ #include "ROS2ServiceBase.h" #include -#include #include namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp index 303b566c67..2064c5b92c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -25,8 +25,8 @@ namespace ROS2SimulationInterfaces AZStd::string entityName = request.entity.c_str(); SimulationInterfaces::EntityState entityState; entityState.m_pose = ROS2::ROS2Conversions::FromROS2Pose(request.state.pose); - entityState.m_twist_angular = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.angular); - entityState.m_twist_linear = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.linear); + entityState.m_twistAngular = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.angular); + entityState.m_twistLinear = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.linear); SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( outcome, &SimulationInterfaces::SimulationEntityManagerRequests::SetEntityState, entityName, entityState); @@ -36,8 +36,8 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } return response; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp index b1642b32ff..9461c236da 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -33,8 +33,8 @@ namespace ROS2SimulationInterfaces } else { - response.result.result = static_cast(transitionResult.GetError().error_code); - response.result.error_message = transitionResult.GetError().error_string.c_str(); + response.result.result = static_cast(transitionResult.GetError().m_errorCode); + response.result.error_message = transitionResult.GetError().m_errorString.c_str(); } return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h index 9222be60e7..c63bba3efd 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h @@ -10,7 +10,6 @@ #include "Services/ROS2ServiceBase.h" #include -#include #include namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp index df46d68550..b714eceb81 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -28,7 +28,7 @@ namespace ROS2SimulationInterfaces const AZStd::string_view entityNamespace{ request.entity_namespace.c_str(), request.entity_namespace.size() }; // Validate entity name - if (!ValidateEntityName(name)) + if (!name.empty() && !ValidateEntityName(name)) { Response response; response.result.result = simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID; @@ -42,7 +42,8 @@ namespace ROS2SimulationInterfaces { Response response; response.result.result = simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID; - response.result.error_message = "Invalid entity namespace. Entity namespaces can only contain alphanumeric characters and forward slashes."; + response.result.error_message = + "Invalid entity namespace. Entity namespaces can only contain alphanumeric characters and forward slashes."; SendResponse(response); return AZStd::nullopt; } @@ -66,8 +67,8 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); }); @@ -82,7 +83,9 @@ namespace ROS2SimulationInterfaces bool SpawnEntityServiceHandler::ValidateFrameName(const AZStd::string& frameName) { - const AZStd::regex frameRegex{ R"(^[a-zA-Z0-9_/]+$)" }; // Entity names can only contain alphanumeric characters and underscores and forward slashes + const AZStd::regex frameRegex{ + R"(^[a-zA-Z0-9_/]+$)" + }; // Entity names can only contain alphanumeric characters and underscores and forward slashes return AZStd::regex_match(frameName, frameRegex); } diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h index e04413edd0..57d7f2f8a8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h @@ -19,7 +19,7 @@ namespace ROS2SimulationInterfaces::Utils AZ::Outcome GetEntityFiltersFromRequest(const RequestType& request) { SimulationInterfaces::EntityFilters filter; - filter.m_filter = request.filters.filter.c_str(); + filter.m_nameFilter = request.filters.filter.c_str(); uint8_t type = request.filters.bounds.type; if (type == simulation_interfaces::msg::Bounds::TYPE_BOX) { @@ -27,8 +27,8 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_BOX' should have exactly 2 points."); } - const auto &p1 = request.filters.bounds.points.front(); - const auto &p2 = request.filters.bounds.points.back(); + const auto& p1 = request.filters.bounds.points.front(); + const auto& p2 = request.filters.bounds.points.back(); if (p1.x > p2.x || p1.y > p2.y || p1.z > p2.z) { return AZ::Failure("Invalid points! The first point should be lower than the second point."); @@ -36,7 +36,7 @@ namespace ROS2SimulationInterfaces::Utils const auto upperRight = ROS2::ROS2Conversions::FromROS2Vector3(p2); const auto lowerLeft = ROS2::ROS2Conversions::FromROS2Vector3(p1); const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(lowerLeft, upperRight); - filter.m_bounds_shape = AZStd::make_shared(aabb.GetExtents()); + filter.m_boundsShape = AZStd::make_shared(aabb.GetExtents()); } else if (type == simulation_interfaces::msg::Bounds::TYPE_CONVEX_HULL) // TYPE_CONVEX_HULL { @@ -44,7 +44,7 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have at least 3 points."); } - filter.m_bounds_shape = AZStd::make_shared(); + filter.m_boundsShape = AZStd::make_shared(); } else if (type == simulation_interfaces::msg::Bounds::TYPE_SPHERE) // TYPE_SPHERE { @@ -52,10 +52,10 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_SPHERE' should have exactly 2 points."); } - filter.m_bounds_shape = AZStd::make_shared(request.filters.bounds.points.back().x); - filter.m_bounds_pose = { ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()), - AZ::Quaternion::CreateIdentity(), - 1.0f }; + filter.m_boundsShape = AZStd::make_shared(request.filters.bounds.points.back().x); + filter.m_boundsPose = { ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()), + AZ::Quaternion::CreateIdentity(), + 1.0f }; } return AZ::Success(AZStd::move(filter)); } diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp index 8708bc73f7..71a2b52c54 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include -#include namespace UnitTest { @@ -146,7 +146,6 @@ namespace UnitTest EXPECT_NE(services.find("/spawn_entity"), services.end()); EXPECT_NE(services.find("/get_simulation_features"), services.end()); EXPECT_NE(services.find("/step_simulation"), services.end()); - } //! Test if the service call succeeds when the entity is found @@ -195,8 +194,8 @@ namespace UnitTest { EXPECT_EQ(name, "test_entity"); FailedResult failedResult; - failedResult.error_code = simulation_interfaces::msg::Result::RESULT_NOT_FOUND, - failedResult.error_string = "FooBar not found."; + failedResult.m_errorCode = simulation_interfaces::msg::Result::RESULT_NOT_FOUND, + failedResult.m_errorString = "FooBar not found."; completedCb(AZ::Failure(failedResult)); })); @@ -230,21 +229,21 @@ namespace UnitTest .WillOnce(::testing::Invoke( [=](const EntityFilters& filter) { - EXPECT_NE(filter.m_bounds_shape, nullptr); - if (filter.m_bounds_shape) + EXPECT_NE(filter.m_boundsShape, nullptr); + if (filter.m_boundsShape) { - EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Sphere); + EXPECT_EQ(filter.m_boundsShape->GetShapeType(), Physics::ShapeType::Sphere); Physics::SphereShapeConfiguration* sphereShape = - azdynamic_cast(filter.m_bounds_shape.get()); + azdynamic_cast(filter.m_boundsShape.get()); EXPECT_EQ(sphereShape->m_radius, 99.0f); EXPECT_EQ(sphereShape->m_scale, AZ::Vector3(1.0f)); } - auto loc = filter.m_bounds_pose.GetTranslation(); + auto loc = filter.m_boundsPose.GetTranslation(); EXPECT_EQ(loc.GetX(), point1.GetX()); EXPECT_EQ(loc.GetY(), point1.GetY()); EXPECT_EQ(loc.GetZ(), point1.GetZ()); - EXPECT_EQ(filter.m_filter, "FooBarFilter"); + EXPECT_EQ(filter.m_nameFilter, "FooBarFilter"); return AZ::Success(EntityNameList{ "FooBar" }); })); @@ -300,17 +299,17 @@ namespace UnitTest .WillOnce(::testing::Invoke( [=](const EntityFilters& filter) { - EXPECT_NE(filter.m_bounds_shape, nullptr); - if (filter.m_bounds_shape) + EXPECT_NE(filter.m_boundsShape, nullptr); + if (filter.m_boundsShape) { - EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Box); + EXPECT_EQ(filter.m_boundsShape->GetShapeType(), Physics::ShapeType::Box); Physics::BoxShapeConfiguration* boxShape = - azdynamic_cast(filter.m_bounds_shape.get()); + azdynamic_cast(filter.m_boundsShape.get()); EXPECT_EQ(boxShape->m_dimensions.GetX(), dims.GetX()); EXPECT_EQ(boxShape->m_dimensions.GetY(), dims.GetY()); EXPECT_EQ(boxShape->m_dimensions.GetZ(), dims.GetZ()); } - auto loc = filter.m_bounds_pose.GetTranslation(); + auto loc = filter.m_boundsPose.GetTranslation(); EXPECT_EQ(loc, AZ::Vector3::CreateZero()); return AZ::Success(EntityNameList{ "FooBar" }); })); @@ -374,10 +373,10 @@ namespace UnitTest SimulationFeaturesAggregatorRequestsMockedHandler mockAggregator; using ::testing::_; auto node = GetRos2Node(); - AZStd::unordered_set features{ + AZStd::unordered_set features{ simulation_interfaces::msg::SimulatorFeatures::SPAWNING, - static_cast(0xFE), // invalid feature, should be ignored - static_cast(0xFF), // invalid feature, should be ignored + static_cast(0xFE), // invalid feature, should be ignored + static_cast(0xFF), // invalid feature, should be ignored }; EXPECT_CALL(mockAggregator, GetSimulationFeatures()) .WillOnce(::testing::Invoke( @@ -412,17 +411,21 @@ namespace UnitTest request->allow_renaming = true; auto future = client->async_send_request(request); - EXPECT_CALL(mock, SpawnEntity(_, _, _, _, _, _)).WillOnce( - ::testing::Invoke( - [](const AZStd::string& name, const AZStd::string& uri, const AZStd::string& entityNamespace, const AZ::Transform& initialPose, const bool allowRename, SpawnCompletedCb completedCb) + EXPECT_CALL(mock, SpawnEntity(_, _, _, _, _, _)) + .WillOnce(::testing::Invoke( + [](const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb) { EXPECT_EQ(name, "valid_name"); EXPECT_EQ(uri, "test_uri"); EXPECT_EQ(entityNamespace, "test_namespace"); EXPECT_TRUE(allowRename); completedCb(AZ::Success("valid_name")); - }) - ); + })); SpinAppSome(); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; @@ -448,8 +451,8 @@ namespace UnitTest ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); - EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID) << "Service call should fail with invalid name."; - + EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID) + << "Service call should fail with invalid name."; } //! Check if service fails when the namespace is invalid @@ -470,7 +473,8 @@ namespace UnitTest ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); - EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID) << "Service call should fail with invalid name."; + EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID) + << "Service call should fail with invalid name."; } TEST_F(SimulationInterfaceROS2TestFixture, SimulationStepsSuccess) diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h index 94c8f263dc..7c7f867d55 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h @@ -17,8 +17,8 @@ namespace UnitTest SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::Handler::BusDisconnect(); } - MOCK_METHOD(void, AddSimulationFeatures, (const AZStd::unordered_set& features), (override)); - MOCK_METHOD(const AZStd::unordered_set, GetSimulationFeatures, (), (const, override)); - MOCK_METHOD(bool, HasFeature, (SimulationFeatures feature), (const, override)); + MOCK_METHOD(void, AddSimulationFeatures, (const AZStd::unordered_set& features), (override)); + MOCK_METHOD(AZStd::unordered_set, GetSimulationFeatures, (), (override)); + MOCK_METHOD(bool, HasFeature, (SimulationFeatureType feature), (override)); }; } // namespace UnitTest diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp index 84b5096c66..50c07eb88f 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp @@ -79,7 +79,7 @@ namespace UnitTest CreateEntityWithStaticBodyComponent("Outside", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); EntityFilters filter; - filter.m_bounds_shape = AZStd::make_shared(2.0f); + filter.m_boundsShape = AZStd::make_shared(2.0f); AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -103,7 +103,7 @@ namespace UnitTest CreateEntityWithStaticBodyComponent("WontMatch", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); EntityFilters filter; - filter.m_filter = "Will.*"; + filter.m_nameFilter = "Will.*"; AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -126,7 +126,7 @@ namespace UnitTest CreateEntityWithStaticBodyComponent("WontMatch", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); EntityFilters filter; - filter.m_filter = "[a-z"; + filter.m_nameFilter = "[a-z"; AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -168,7 +168,7 @@ namespace UnitTest EXPECT_GT(deltaPos.GetLength(), 0.0f); // check if entity has velocity - EXPECT_GT(stateAfter.m_twist_linear.GetLength(), 0.0f); + EXPECT_GT(stateAfter.m_twistLinear.GetLength(), 0.0f); DeleteEntity(entityId1); } @@ -182,8 +182,7 @@ namespace UnitTest deletionWasCompleted = true; EXPECT_TRUE(result.IsSuccess()); }; - SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); TickApp(100); EXPECT_TRUE(deletionWasCompleted); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp index 50058b99fe..28233734e4 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -78,7 +78,13 @@ namespace UnitTest constexpr bool allowRename = false; SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, completedCb); + &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); @@ -92,7 +98,13 @@ namespace UnitTest completed2 = true; }; SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, failedSpawnCompletedCb); + &SimulationEntityManagerRequestBus::Events::SpawnEntity, + entityName, + uri, + entityNamespace, + initialPose, + allowRename, + failedSpawnCompletedCb); EXPECT_TRUE(completed2); // list simulation entities @@ -136,8 +148,8 @@ namespace UnitTest // Check if entity was teleported by setting the new state, we use a filter to check if the entity is at the new position EntityFilters filter; - filter.m_bounds_shape = AZStd::make_shared(2.0f); - filter.m_bounds_pose = AZ::Transform::CreateTranslation(AZ::Vector3(1000.0f, 0.0f, 0.0f)); + filter.m_boundsShape = AZStd::make_shared(2.0f); + filter.m_boundsPose = AZ::Transform::CreateTranslation(AZ::Vector3(1000.0f, 0.0f, 0.0f)); AZ::Outcome entitiesFiltered; SimulationEntityManagerRequestBus::BroadcastResult( entitiesFiltered, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -157,7 +169,9 @@ namespace UnitTest EXPECT_EQ(getNumberOfEntities(), 0); // spawn 3 entities of entities and despawn all of them - SpawnCompletedCb cb = [&](const AZ::Outcome& result){}; + SpawnCompletedCb cb = [&](const AZ::Outcome& result) + { + }; SimulationEntityManagerRequestBus::Broadcast( &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity1", uri, entityNamespace, initialPose, false, cb); SimulationEntityManagerRequestBus::Broadcast( @@ -174,13 +188,11 @@ namespace UnitTest deletionWasCompleted = true; EXPECT_TRUE(result.IsSuccess()); }; - SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); TickApp(100); EXPECT_TRUE(deletionWasCompleted); EXPECT_EQ(getNumberOfEntities(), 0); - } } // namespace UnitTest