diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index a7654f17a8..617b7c95e7 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -42,6 +42,25 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -DROS_DISTRO=${ROS_DISTRO} -P ${CMAKE_CURRENT_SOURCE_DIR}/checkROS2Distribution.cmake ) +# Gazebo messages are optional, so we will only add the dependents if the package is found. +# The gazebo_msgs package is EOL and will not be available in ROS 2 Kilted Kaiju. +# If you need to use ContactSensor and/or ROS2 Spawner, please consider building gazebo_msgs from the source. +find_package(gazebo_msgs QUIET) +if (gazebo_msgs_FOUND) + message(STATUS "Found gazebo_msgs package, enabling legacy features like ContactSensor Component and ROS2 Spawner Component") + SET (WITH_GAZEBO_MSGS TRUE) + if(NOT (ROS_DISTRO STREQUAL "humble" OR ROS_DISTRO STREQUAL "jazzy")) + message(WARNING + "The support for deprecated gazebo_msgs package is not supported in ROS 2 Kilted or newer. " + "Please consider migration to Simulation Interfaces. " + "If you do not intend to use Gazebo messages, please make sure that this package is not sourced in your environment." ) + + endif() +else() + message(STATUS "Could not find gazebo_msgs package, disabling legacy features like ContactSensor Component and ROS2 Spawner Component") + SET(WITH_GAZEBO_MSGS FALSE) +endif() + # Add the ROS2.Static target # Note: We include the common files and the platform specific files which are set in ros2_common_files.cmake # and in ${pal_dir}/ros2_${PAL_PLATFORM_NAME_LOWERCASE}_files.cmake @@ -71,7 +90,12 @@ ly_add_target( Gem::LevelGeoreferencing.API ) -target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs vision_msgs control_msgs) +target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs vision_msgs control_msgs) + +if (WITH_GAZEBO_MSGS) + target_depends_on_ros2_package(${gem_name}.Static gazebo_msgs REQUIRED) + target_compile_definitions(${gem_name}.Static PUBLIC "WITH_GAZEBO_MSGS") +endif() ly_add_target( NAME ${gem_name}.API HEADERONLY diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ITimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/ITimeSource.h index 7868543734..427b347904 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ITimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ITimeSource.h @@ -8,6 +8,8 @@ #pragma once #include +#include +#include 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 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; diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h index 18f600c4e0..a4725e9d60 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h @@ -8,8 +8,10 @@ #pragma once #include "ITimeSource.h" +#include #include #include +#include #include #include @@ -21,7 +23,6 @@ namespace ROS2 //! the /use_sim_time parameter set to true. class ROS2Clock { - public: ROS2Clock(); ROS2Clock(AZStd::unique_ptr timeSource, bool publishClock); @@ -32,6 +33,11 @@ namespace ROS2 builtin_interfaces::msg::Time GetROSTimestamp() const; + //! 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. + AZ::Outcome 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(); diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h index 33c2616868..599a4ace3f 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h @@ -25,6 +25,12 @@ namespace ROS2 //! Get ROS 2 time as ROS2 message. //! @see ROS2Requests::GetROSTimestamp() for more details. virtual builtin_interfaces::msg::Time GetROSTimestamp() const override; + + //! 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 AdjustTime(const builtin_interfaces::msg::Time& time) override; + }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h index f823befaa0..e2dbf5731c 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h @@ -14,7 +14,7 @@ namespace ROS2 //! The RealTimeSource starts from 0 at the start of the simulation. //! This time source could be affected by the jitter in the data, simulation //! computations or other similar events. On the other hand RealTimeSource - //! can remain consistent with the other independent clocks if it is synchronised + //! can remain consistent with the other independent clocks if it is synchronized //! (e.g. through NTP). class RealTimeSource : public ITimeSource { @@ -22,8 +22,13 @@ namespace ROS2 virtual ~RealTimeSource() = default; // ITimeSource overrides ... - virtual void Activate() override {}; - virtual void Deactivate() override {}; + virtual void Activate() override{}; + virtual void Deactivate() override{}; + + //! 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 AdjustTime(const builtin_interfaces::msg::Time& time) override; //! Get simulation time as ROS 2 message. //! @see ROS2Requests::GetROSTimestamp() for more details. diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h index 0a91b71ece..6f958e7643 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h @@ -30,6 +30,11 @@ namespace ROS2 virtual void Activate() override; virtual void Deactivate() override; + //! 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 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; diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.cpp index e820b5649e..faf07b6b25 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.cpp @@ -41,7 +41,11 @@ namespace ROS2 void ROS2SystemCameraComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { required.push_back(AZ_CRC_CE("ROS2Service")); - required.push_back(AZ_CRC_CE("RPISystem")); + } + + void ROS2SystemCameraComponent::GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + dependent.push_back(AZ_CRC_CE("RPISystem")); } void ROS2SystemCameraComponent::InitPassTemplateMappingsHandler() diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.h index e1420b3be8..2118d7fc04 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSystemComponent.h @@ -24,6 +24,7 @@ namespace ROS2 static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); void InitPassTemplateMappingsHandler(); diff --git a/Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp b/Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp index 35482b8065..bc056063c0 100644 --- a/Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp +++ b/Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp @@ -58,4 +58,9 @@ namespace ROS2 msg.clock = GetROSTimestamp(); m_clockPublisher->publish(msg); } + + AZ::Outcome ROS2Clock::AdjustTime(const builtin_interfaces::msg::Time & time) const + { + return m_timeSource->AdjustTime(time); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Clock/ROS2TimeSource.cpp b/Gems/ROS2/Code/Source/Clock/ROS2TimeSource.cpp index fc94362759..7e9ef139c8 100644 --- a/Gems/ROS2/Code/Source/Clock/ROS2TimeSource.cpp +++ b/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 ROS2TimeSource::AdjustTime(const builtin_interfaces::msg::Time& time) + { + return AZ::Failure(AZStd::string("ROS2TimeSource does not support setting a specific time.")); + } + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Clock/RealTimeSource.cpp b/Gems/ROS2/Code/Source/Clock/RealTimeSource.cpp index 5f99be72a4..ed12d92a0f 100644 --- a/Gems/ROS2/Code/Source/Clock/RealTimeSource.cpp +++ b/Gems/ROS2/Code/Source/Clock/RealTimeSource.cpp @@ -33,4 +33,9 @@ namespace ROS2 return 0; } } + + AZ::Outcome RealTimeSource::AdjustTime(const builtin_interfaces::msg::Time& time) + { + return AZ::Failure(AZStd::string("RealTimeSource does not support setting a specific time.")); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Clock/SimulationTimeSource.cpp b/Gems/ROS2/Code/Source/Clock/SimulationTimeSource.cpp index 207961e1b5..b3a4fe35a8 100644 --- a/Gems/ROS2/Code/Source/Clock/SimulationTimeSource.cpp +++ b/Gems/ROS2/Code/Source/Clock/SimulationTimeSource.cpp @@ -83,4 +83,11 @@ namespace ROS2 timeStamp.nanosec = static_cast((m_elapsed - timeStamp.sec) * 1e9); return timeStamp; } + + AZ::Outcome SimulationTimeSource::AdjustTime(const builtin_interfaces::msg::Time& time) + { + const double timeSec = static_cast(time.sec) + static_cast(time.nanosec) * 1e-9; + m_elapsed = timeSec; + return AZ::Success(); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/ROS2EditorModule.cpp b/Gems/ROS2/Code/Source/ROS2EditorModule.cpp index f28b4ace9f..0f7a41fc60 100644 --- a/Gems/ROS2/Code/Source/ROS2EditorModule.cpp +++ b/Gems/ROS2/Code/Source/ROS2EditorModule.cpp @@ -17,10 +17,11 @@ #include #include #include +#include +#ifdef WITH_GAZEBO_MSGS #include #include -#include - +#endif void InitROS2Resources() { // Registration of Qt (ROS2.qrc) resources @@ -46,8 +47,10 @@ namespace ROS2 LidarRegistrarEditorSystemComponent::CreateDescriptor(), ROS2RobotImporterEditorSystemComponent::CreateDescriptor(), ROS2CameraSensorEditorComponent::CreateDescriptor(), +#ifdef WITH_GAZEBO_MSGS ROS2SpawnerEditorComponent::CreateDescriptor(), ROS2SpawnPointEditorComponent::CreateDescriptor(), +#endif SdfAssetBuilderSystemComponent::CreateDescriptor(), JointsManipulationEditorComponent::CreateDescriptor(), JointsPositionsEditorComponent::CreateDescriptor(), diff --git a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h index afc2a32711..c57289745a 100644 --- a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h +++ b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h @@ -12,7 +12,6 @@ #include #include #include -#include #include #include #include @@ -39,14 +38,16 @@ #include #include #include -#include -#include #include #include #include #include #include - +#ifdef WITH_GAZEBO_MSGS +#include +#include +#include +#endif namespace ROS2 { class ROS2ModuleInterface : public AZ::Module @@ -74,14 +75,11 @@ namespace ROS2 ROS2WheelOdometryComponent::CreateDescriptor(), ROS2FrameComponent::CreateDescriptor(), ROS2RobotControlComponent::CreateDescriptor(), - ROS2CameraSensorComponent::CreateDescriptor(), ROS2ImageEncodingConversionComponent::CreateDescriptor(), AckermannControlComponent::CreateDescriptor(), RigidBodyTwistControlComponent::CreateDescriptor(), SkidSteeringControlComponent::CreateDescriptor(), ROS2CameraSensorComponent::CreateDescriptor(), - ROS2SpawnerComponent::CreateDescriptor(), - ROS2SpawnPointComponent::CreateDescriptor(), VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(), VehicleDynamics::WheelControllerComponent::CreateDescriptor(), VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(), @@ -96,9 +94,13 @@ namespace ROS2 GripperActionServerComponent::CreateDescriptor(), VacuumGripperComponent::CreateDescriptor(), FingerGripperComponent::CreateDescriptor(), - ROS2ContactSensorComponent::CreateDescriptor(), FollowingCameraComponent::CreateDescriptor(), ClassSegmentationConfigurationComponent::CreateDescriptor(), +#ifdef WITH_GAZEBO_MSGS + ROS2SpawnerComponent::CreateDescriptor(), + ROS2SpawnPointComponent::CreateDescriptor(), + ROS2ContactSensorComponent::CreateDescriptor(), +#endif }); } diff --git a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp index 98e1f1c272..7169c867af 100644 --- a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp @@ -81,7 +81,6 @@ namespace ROS2 void ROS2SystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { required.push_back(AZ_CRC("AssetDatabaseService", 0x3abf5601)); - required.push_back(AZ_CRC("RPISystem", 0xf2add773)); } void ROS2SystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index d4b8a6ebe4..a61fb23ebe 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -84,10 +84,6 @@ set(FILES Source/RobotImporter/Utils/TypeConversions.cpp Source/RobotImporter/Utils/TypeConversions.h Source/ROS2GemUtilities.cpp - Source/Spawner/ROS2SpawnerEditorComponent.cpp - Source/Spawner/ROS2SpawnerEditorComponent.h - Source/Spawner/ROS2SpawnPointEditorComponent.cpp - Source/Spawner/ROS2SpawnPointEditorComponent.h Source/SdfAssetBuilder/SdfAssetBuilder.cpp Source/SdfAssetBuilder/SdfAssetBuilder.h Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp @@ -101,3 +97,14 @@ set(FILES Source/SystemComponents/ROS2EditorSystemComponent.cpp Source/SystemComponents/ROS2EditorSystemComponent.h ) + +# optional, legacy features compilation +if (WITH_GAZEBO_MSGS) + list(APPEND FILES + Source/Spawner/ROS2SpawnerEditorComponent.cpp + Source/Spawner/ROS2SpawnerEditorComponent.h + Source/Spawner/ROS2SpawnPointEditorComponent.cpp + Source/Spawner/ROS2SpawnPointEditorComponent.h + ) +endif () + diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index 988dab2860..ebd5ec1d8b 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/Gems/ROS2/Code/ros2_files.cmake @@ -33,8 +33,6 @@ set(FILES Source/Communication/QoS.cpp Source/Communication/PublisherConfiguration.cpp Source/Communication/TopicConfiguration.cpp - Source/ContactSensor/ROS2ContactSensorComponent.cpp - Source/ContactSensor/ROS2ContactSensorComponent.h Source/Frame/NamespaceConfiguration.cpp Source/Frame/ROS2FrameComponent.cpp Source/Frame/ROS2FrameConfiguration.cpp @@ -130,14 +128,6 @@ set(FILES Source/SimulationUtils/FollowingCameraConfiguration.h Source/SimulationUtils/FollowingCameraComponent.cpp Source/SimulationUtils/FollowingCameraComponent.h - Source/Spawner/ROS2SpawnerComponent.cpp - Source/Spawner/ROS2SpawnerComponent.h - Source/Spawner/ROS2SpawnPointComponent.cpp - Source/Spawner/ROS2SpawnPointComponent.h - Source/Spawner/ROS2SpawnerComponentController.cpp - Source/Spawner/ROS2SpawnerComponentController.h - Source/Spawner/ROS2SpawnPointComponentController.cpp - Source/Spawner/ROS2SpawnPointComponentController.h Source/SystemComponents/ROS2SystemComponent.cpp Source/SystemComponents/ROS2SystemComponent.h Source/Utilities/ArticulationsUtilities.cpp @@ -178,3 +168,19 @@ set(FILES Source/VehicleDynamics/WheelControllerComponent.h Source/VehicleDynamics/WheelDynamicsData.h ) + +# optional, legacy features compilation +if (WITH_GAZEBO_MSGS) + list(APPEND FILES + Source/ContactSensor/ROS2ContactSensorComponent.cpp + Source/ContactSensor/ROS2ContactSensorComponent.h + Source/Spawner/ROS2SpawnerComponent.cpp + Source/Spawner/ROS2SpawnerComponent.h + Source/Spawner/ROS2SpawnPointComponent.cpp + Source/Spawner/ROS2SpawnPointComponent.h + Source/Spawner/ROS2SpawnerComponentController.cpp + Source/Spawner/ROS2SpawnerComponentController.h + Source/Spawner/ROS2SpawnPointComponentController.cpp + Source/Spawner/ROS2SpawnPointComponentController.h + ) +endif () diff --git a/Gems/ROS2/Code/ros2_target_depends.cmake b/Gems/ROS2/Code/ros2_target_depends.cmake index 71b928beec..ccb1a6d337 100644 --- a/Gems/ROS2/Code/ros2_target_depends.cmake +++ b/Gems/ROS2/Code/ros2_target_depends.cmake @@ -6,12 +6,16 @@ function(target_depends_on_ros2_package TARGET_NAME) list(GET ARGN 0 _package) find_package(${ARGN}) - include(${${_package}_DIR}/${_package}Config.cmake OPTIONAL) - if (${${_package}_FOUND_AMENT_PACKAGE}) - message(DEBUG "Package ${_package} was found (${${_package}_DIR}) version ${${_package}_VERSION} targets : ${${_package}_TARGETS}") - target_link_libraries(${TARGET_NAME} PUBLIC ${${_package}_TARGETS}) + if (${${_package}_FOUND}) + include(${${_package}_DIR}/${_package}Config.cmake OPTIONAL) + if (${${_package}_FOUND_AMENT_PACKAGE}) + message(DEBUG "Package ${_package} was found (${${_package}_DIR}) version ${${_package}_VERSION} targets : ${${_package}_TARGETS}") + target_link_libraries(${TARGET_NAME} PUBLIC ${${_package}_TARGETS}) + else () + message(FATAL_ERROR "Package ${_package} was found (${${_package}_DIR}), but package is not an Ament package.") + endif () else () - message(FATAL_ERROR "Package ${_package} was found (${${_package}_DIR}), but package is not an Ament package.") + message(DEBUG "Package ${_package} was not found.") endif () endfunction() diff --git a/Gems/SimulationInterfaces/.clang-format b/Gems/SimulationInterfaces/.clang-format new file mode 100644 index 0000000000..fddf244031 --- /dev/null +++ b/Gems/SimulationInterfaces/.clang-format @@ -0,0 +1,61 @@ +Language: Cpp + +AccessModifierOffset: -4 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Right +AlignOperands: false +AlignTrailingComments: false +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortFunctionsOnASingleLine: None +AllowShortLambdasOnASingleLine: None +AlwaysBreakAfterReturnType: None +AlwaysBreakTemplateDeclarations: true +BinPackArguments: false +BinPackParameters: false +BreakBeforeBraces: Custom +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + BeforeLambdaBody: true + AfterStruct: true + BeforeElse: true + SplitEmptyFunction: true +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeComma +BreakInheritanceList: BeforeComma +ColumnLimit: 140 +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false +FixNamespaceComments: true +IncludeBlocks: Preserve +IndentCaseBlocks: true +IndentCaseLabels: false +IndentPPDirectives: None +IndentWidth: 4 +KeepEmptyLinesAtTheStartOfBlocks: false +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: All +PenaltyReturnTypeOnItsOwnLine: 1000 +PointerAlignment: Left +SortIncludes: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyParentheses: false +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +Standard: c++17 +UseTab: Never \ No newline at end of file diff --git a/Gems/SimulationInterfaces/Assets/SampleAsset/TestSimulationEntity.prefab b/Gems/SimulationInterfaces/Assets/SampleAsset/TestSimulationEntity.prefab new file mode 100755 index 0000000000..37351e162f --- /dev/null +++ b/Gems/SimulationInterfaces/Assets/SampleAsset/TestSimulationEntity.prefab @@ -0,0 +1,137 @@ +{ + "ContainerEntity": { + "Id": "ContainerEntity", + "Name": "TestSimulationEntity", + "Components": { + "EditorDisabledCompositionComponent": { + "$type": "EditorDisabledCompositionComponent", + "Id": 13342689700908152991 + }, + "EditorEntityIconComponent": { + "$type": "EditorEntityIconComponent", + "Id": 8584796089140035515 + }, + "EditorEntitySortComponent": { + "$type": "EditorEntitySortComponent", + "Id": 17941228157017035486, + "Child Entity Order": [ + "Entity_[2555577822056]" + ] + }, + "EditorInspectorComponent": { + "$type": "EditorInspectorComponent", + "Id": 16387168034938951622 + }, + "EditorLockComponent": { + "$type": "EditorLockComponent", + "Id": 5868718090530092403 + }, + "EditorOnlyEntityComponent": { + "$type": "EditorOnlyEntityComponent", + "Id": 10685569871653723108 + }, + "EditorPendingCompositionComponent": { + "$type": "EditorPendingCompositionComponent", + "Id": 13486227769083255940 + }, + "EditorPrefabComponent": { + "$type": "EditorPrefabComponent", + "Id": 9974721317876760075 + }, + "EditorVisibilityComponent": { + "$type": "EditorVisibilityComponent", + "Id": 18186949152932686714 + }, + "TransformComponent": { + "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", + "Id": 13473127497085401640, + "Parent Entity": "" + } + } + }, + "Entities": { + "Entity_[2555577822056]": { + "Id": "Entity_[2555577822056]", + "Name": "TestSimulationEntity", + "Components": { + "EditorBoxShapeComponent": { + "$type": "EditorBoxShapeComponent", + "Id": 17815707635495439549, + "GameView": true + }, + "EditorColliderComponent": { + "$type": "EditorColliderComponent", + "Id": 7164453177612248133, + "ColliderConfiguration": { + "MaterialSlots": { + "Slots": [ + { + "Name": "Entire object" + } + ] + } + }, + "ShapeConfiguration": { + "ShapeType": 0 + } + }, + "EditorDisabledCompositionComponent": { + "$type": "EditorDisabledCompositionComponent", + "Id": 14547453815095477983 + }, + "EditorEntityIconComponent": { + "$type": "EditorEntityIconComponent", + "Id": 11928217918748032436 + }, + "EditorEntitySortComponent": { + "$type": "EditorEntitySortComponent", + "Id": 16175661524427525128 + }, + "EditorInspectorComponent": { + "$type": "EditorInspectorComponent", + "Id": 7165286389418841970 + }, + "EditorLockComponent": { + "$type": "EditorLockComponent", + "Id": 18134844107097174864 + }, + "EditorOnlyEntityComponent": { + "$type": "EditorOnlyEntityComponent", + "Id": 3157917745503616515 + }, + "EditorPendingCompositionComponent": { + "$type": "EditorPendingCompositionComponent", + "Id": 10603658429884530304 + }, + "EditorRigidBodyComponent": { + "$type": "EditorRigidBodyComponent", + "Id": 11637898985208574279, + "Configuration": { + "entityId": "", + "Mass": 523.5988159179688, + "Inertia tensor": [ + 52.35987854003906, + 0.0, + 0.0, + 0.0, + 52.35987854003906, + 0.0, + 0.0, + 0.0, + 52.35987854003906 + ] + } + }, + "EditorVisibilityComponent": { + "$type": "EditorVisibilityComponent", + "Id": 6275443825220336439 + }, + "TransformComponent": { + "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", + "Id": 14994870681931426785, + "Parent Entity": "ContainerEntity" + } + } + } + } +} \ No newline at end of file diff --git a/Gems/SimulationInterfaces/CMakeLists.txt b/Gems/SimulationInterfaces/CMakeLists.txt new file mode 100644 index 0000000000..d759cbf1a3 --- /dev/null +++ b/Gems/SimulationInterfaces/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +o3de_gem_setup("SimulationInterfaces") + +ly_add_external_target_path(${CMAKE_CURRENT_SOURCE_DIR}/3rdParty) + +add_subdirectory(Code) diff --git a/Gems/SimulationInterfaces/Code/CMakeLists.txt b/Gems/SimulationInterfaces/Code/CMakeLists.txt new file mode 100644 index 0000000000..2ec9280728 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/CMakeLists.txt @@ -0,0 +1,324 @@ +# 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 +# + +# Currently we are in the Code folder: ${CMAKE_CURRENT_LIST_DIR} +# Get the platform specific folder ${pal_dir} for the current folder: ${CMAKE_CURRENT_LIST_DIR}/Platform/${PAL_PLATFORM_NAME} +# Note: o3de_pal_dir will take care of the details for us, as this may be a restricted platform +# in which case it will see if that platform is present here or in the restricted folder. +# i.e. It could here in our gem : Gems/SimulationInterfaces/Code/Platform/ or +# //Gems/SimulationInterfaces/Code +o3de_pal_dir(pal_dir ${CMAKE_CURRENT_LIST_DIR}/Platform/${PAL_PLATFORM_NAME} "${gem_restricted_path}" "${gem_path}" "${gem_parent_relative_path}") + +# Now that we have the platform abstraction layer (PAL) folder for this folder, thats where we will find the +# traits for this platform. Traits for a platform are defines for things like whether or not something in this gem +# is supported by this platform. +include(${pal_dir}/PAL_${PAL_PLATFORM_NAME_LOWERCASE}.cmake) + +# Check to see if building the Gem Modules are supported for the current platform +if(NOT PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED) + return() +endif() + +# The ${gem_name}.API target declares the common interface that users of this gem should depend on in their targets +ly_add_target( + NAME ${gem_name}.API INTERFACE + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_api_files.cmake + ${pal_dir}/simulationinterfaces_api_files.cmake + INCLUDE_DIRECTORIES + INTERFACE + Include + BUILD_DEPENDENCIES + INTERFACE + AZ::AzCore +) + +# The ${gem_name}.Private.Object target is an internal target +# It should not be used outside of this Gems CMakeLists.txt +ly_add_target( + NAME ${gem_name}.Private.Object STATIC + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_private_files.cmake + ${pal_dir}/simulationinterfaces_private_files.cmake + TARGET_PROPERTIES + O3DE_PRIVATE_TARGET TRUE + INCLUDE_DIRECTORIES + PRIVATE + Include + Source + BUILD_DEPENDENCIES + PUBLIC + AZ::AzCore + AZ::AzFramework + PRIVATE + Gem::ROS2.Static + Gem::DebugDraw.API +) +target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces rclcpp_action) + +# Here add ${gem_name} target, it depends on the Private Object library and Public API interface +ly_add_target( + NAME ${gem_name} ${PAL_TRAIT_MONOLITHIC_DRIVEN_MODULE_TYPE} + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_shared_files.cmake + ${pal_dir}/simulationinterfaces_shared_files.cmake + INCLUDE_DIRECTORIES + PUBLIC + Include + PRIVATE + Source + BUILD_DEPENDENCIES + PUBLIC + Gem::${gem_name}.API + PRIVATE + Gem::${gem_name}.Private.Object +) + +# Include the gem name into the Client Module source file +# for use with the AZ_DECLARE_MODULE_CLASS macro +# This is to allow renaming of the gem to also cause +# the CreateModuleClass_Gem_ function which +# is used to bootstrap the gem in monolithic builds to link to the new gem name +ly_add_source_properties( +SOURCES + Source/Clients/SimulationInterfacesModule.cpp +PROPERTY COMPILE_DEFINITIONS + VALUES + O3DE_GEM_NAME=${gem_name} + O3DE_GEM_VERSION=${gem_version}) + +# By default, we will specify that the above target ${gem_name} would be used by +# Client and Server type targets when this gem is enabled. If you don't want it +# active in Clients or Servers by default, delete one of both of the following lines: +ly_create_alias(NAME ${gem_name}.Clients NAMESPACE Gem TARGETS Gem::${gem_name}) +ly_create_alias(NAME ${gem_name}.Servers NAMESPACE Gem TARGETS Gem::${gem_name}) +ly_create_alias(NAME ${gem_name}.Unified NAMESPACE Gem TARGETS Gem::${gem_name}) + +# For the Client and Server variants of ${gem_name} Gem, an alias to the ${gem_name}.API target will be made +ly_create_alias(NAME ${gem_name}.Clients.API NAMESPACE Gem TARGETS Gem::${gem_name}.API) +ly_create_alias(NAME ${gem_name}.Servers.API NAMESPACE Gem TARGETS Gem::${gem_name}.API) +ly_create_alias(NAME ${gem_name}.Unified.API NAMESPACE Gem TARGETS Gem::${gem_name}.API) + +# Add in CMake dependencies for each gem dependency listed in this gem's gem.json file +# for the Clients, Servers, Unified gem variants +o3de_add_variant_dependencies_for_gem_dependencies(GEM_NAME ${gem_name} VARIANTS Clients Servers Unified) + +# If we are on a host platform, we want to add the host tools targets like the ${gem_name}.Editor MODULE target +if(PAL_TRAIT_BUILD_HOST_TOOLS) + # The ${gem_name}.Editor.API target can be used by other gems that want to interact with the ${gem_name}.Editor module + ly_add_target( + NAME ${gem_name}.Editor.API INTERFACE + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_editor_api_files.cmake + ${pal_dir}/simulationinterfaces_editor_api_files.cmake + INCLUDE_DIRECTORIES + INTERFACE + Include + BUILD_DEPENDENCIES + INTERFACE + AZ::AzToolsFramework + ) + + # The ${gem_name}.Editor.Private.Object target is an internal target + # which is only to be used by this gems CMakeLists.txt and any subdirectories + # Other gems should not use this target + ly_add_target( + NAME ${gem_name}.Editor.Private.Object STATIC + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_editor_private_files.cmake + TARGET_PROPERTIES + O3DE_PRIVATE_TARGET TRUE + INCLUDE_DIRECTORIES + PRIVATE + Include + Source + BUILD_DEPENDENCIES + + PUBLIC + AZ::AzToolsFramework + Gem::ROS2.API + ${gem_name}.Private.Object + ) + + ly_add_target( + NAME ${gem_name}.Editor GEM_MODULE + NAMESPACE Gem + AUTOMOC + FILES_CMAKE + simulationinterfaces_editor_shared_files.cmake + INCLUDE_DIRECTORIES + PRIVATE + Source + PUBLIC + Include + BUILD_DEPENDENCIES + PUBLIC + Gem::${gem_name}.Editor.API + PRIVATE + Gem::${gem_name}.Editor.Private.Object + + + ) + + # Include the gem name into the Editor Module source file + # for use with the AZ_DECLARE_MODULE_CLASS macro + # This is to allow renaming of the gem to also cause + # the CreateModuleClass_Gem_ function which + # is used to bootstrap the gem in monolithic builds to link to the new gem name + ly_add_source_properties( + SOURCES + Source/Tools/SimulationInterfacesEditorModule.cpp + PROPERTY COMPILE_DEFINITIONS + VALUES + O3DE_GEM_NAME=${gem_name} + O3DE_GEM_VERSION=${gem_version}) + + # By default, we will specify that the above target ${gem_name} would be used by + # Tool and Builder type targets when this gem is enabled. If you don't want it + # active in Tools or Builders by default, delete one of both of the following lines: + ly_create_alias(NAME ${gem_name}.Tools NAMESPACE Gem TARGETS Gem::${gem_name}.Editor) + ly_create_alias(NAME ${gem_name}.Builders NAMESPACE Gem TARGETS Gem::${gem_name}.Editor) + + # For the Tools and Builders variants of ${gem_name} Gem, an alias to the ${gem_name}.Editor API target will be made + ly_create_alias(NAME ${gem_name}.Tools.API NAMESPACE Gem TARGETS Gem::${gem_name}.Editor.API) + ly_create_alias(NAME ${gem_name}.Builders.API NAMESPACE Gem TARGETS Gem::${gem_name}.Editor.API) + + # Add in CMake dependencies for each gem dependency listed in this gem's gem.json file + # for the Tools and Builders gem variants + o3de_add_variant_dependencies_for_gem_dependencies(GEM_NAME ${gem_name} VARIANTS Tools Builders) +endif() + +################################################################################ +# Tests +################################################################################ +# See if globally, tests are supported +if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) + # We globally support tests, see if we support tests on this platform for ${gem_name}.Tests + if(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED) + # We support ${gem_name}.Tests on this platform, add dependency on the Private Object target + ly_add_target( + NAME ${gem_name}.Tests ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_tests_files.cmake + INCLUDE_DIRECTORIES + PRIVATE + Tests + Source + Include + BUILD_DEPENDENCIES + PRIVATE + AZ::AzTest + AZ::AzFramework + Gem::${gem_name}.Private.Object + ) + + # Add ${gem_name}.Tests to googletest + ly_add_googletest( + NAME Gem::${gem_name}.Tests + ) + endif() + + # If we are a host platform we want to add tools test like editor tests here + if(PAL_TRAIT_BUILD_HOST_TOOLS) + # We are a host platform, see if Editor tests are supported on this platform + if(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED) + # We support ${gem_name}.Editor.Tests on this platform, add ${gem_name}.Editor.Tests target which depends on + # private ${gem_name}.Editor.Private.Object target + ly_add_target( + NAME ${gem_name}.Editor.Tests ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_editor_tests_files.cmake + INCLUDE_DIRECTORIES + PRIVATE + Tests + Source + Include + BUILD_DEPENDENCIES + PRIVATE + AZ::AzTest + AZ::AzTestShared + AZ::AzToolsFramework + Legacy::CryCommon + Legacy::EditorCommon + Legacy::Editor.Headers + AZ::AzManipulatorTestFramework.Static + Gem::${gem_name}.API + Gem::${gem_name}.Editor.Private.Object + ) + + # Add ${gem_name}.Editor.Tests to googletest + ly_add_googletest( + NAME Gem::${gem_name}.Editor.Tests + ) + + ly_add_target( + NAME ${gem_name}.TestApp ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE + simulationinterfaces_editor_app_test.cmake + INCLUDE_DIRECTORIES + PRIVATE + Tests + Source + Include + BUILD_DEPENDENCIES + PRIVATE + AZ::AzTest + AZ::AzTestShared + AZ::AzToolsFramework + Legacy::CryCommon + Legacy::EditorCommon + Legacy::Editor.Headers + AZ::AzManipulatorTestFramework.Static + Gem::${gem_name}.API + Gem::${gem_name}.Editor.Private.Object + ) + + # Add ${gem_name}.Editor.Tests to googletest + ly_add_googletest( + NAME Gem::${gem_name}.TestApp + ) + + ly_add_target( + NAME ${gem_name}.ROS2Tests ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE + ros2_simulationinterfaces_tests_files.cmake + INCLUDE_DIRECTORIES + PRIVATE + Tests + Source + Include + BUILD_DEPENDENCIES + PRIVATE + AZ::AzTest + AZ::AzTestShared + AZ::AzToolsFramework + Legacy::CryCommon + Legacy::EditorCommon + Legacy::Editor.Headers + AZ::AzManipulatorTestFramework.Static + Gem::${gem_name}.API + Gem::${gem_name}.Editor.Private.Object + Gem::SimulationInterfaces.API + Gem::ROS2.Static + Gem::${gem_name}.Editor.Private.Object + ) + + # Add ${gem_name}.Editor.Tests to googletest + ly_add_googletest( + NAME Gem::${gem_name}.ROS2Tests + ) + endif() + endif() +endif() diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h new file mode 100644 index 0000000000..50b04055f3 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h @@ -0,0 +1,45 @@ +/* + * 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 "ROS2SimulationInterfacesTypeIds.h" + +#include +#include +#include +#include +namespace ROS2SimulationInterfaces +{ + using SimulationFeatureType = simulation_interfaces::msg::SimulatorFeatures::_features_type::value_type; + class ROS2SimulationInterfacesRequests + { + public: + AZ_RTTI(ROS2SimulationInterfacesRequests, ROS2SimulationInterfacesRequestBusTypeId); + virtual ~ROS2SimulationInterfacesRequests() = default; + + //! Returns set of simulation features available in ROS2SimulationInterfaces Gem + //! SimulationFeatureType follows definition available at: + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + virtual AZStd::unordered_set GetSimulationFeatures() = 0; + }; + + class ROS2SimulationInterfacesRequestBusTraits : public AZ::EBusTraits + { + public: + ////////////////////////////////////////////////////////////////////////// + // EBusTraits overrides + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + ////////////////////////////////////////////////////////////////////////// + }; + + using ROS2SimulationInterfacesRequestBus = AZ::EBus; + using ROS2SimulationInterfacesRequestBusInterface = AZ::Interface; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h new file mode 100644 index 0000000000..93240dde4c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h @@ -0,0 +1,26 @@ +/* + * 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 + +namespace ROS2SimulationInterfaces +{ + // System Component TypeIds + inline constexpr const char* ROS2SimulationInterfacesSystemComponentTypeId = "{9CD6E9FA-5C17-454C-B8FA-033DF572B160}"; + inline constexpr const char* ROS2SimulationInterfacesEditorSystemComponentTypeId = "{AF5BE964-4B5F-49A4-A308-0B6077E5BB26}"; + + // Module derived classes TypeIds + inline constexpr const char* ROS2SimulationInterfacesModuleInterfaceTypeId = "{2F1ED7E1-6808-420D-939F-7D5C9CBFB3C9}"; + inline constexpr const char* ROS2SimulationInterfacesModuleTypeId = "{4002B625-F939-44AC-845B-820B20AFC6C5}"; + // The Editor Module by default is mutually exclusive with the Client Module + // so they use the Same TypeId + inline constexpr const char* ROS2SimulationInterfacesEditorModuleTypeId = ROS2SimulationInterfacesModuleTypeId; + + // API TypeIds + inline constexpr const char* ROS2SimulationInterfacesRequestBusTypeId = "{00D08870-E329-4BD7-BB8C-F67FE369DE92}"; +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h new file mode 100644 index 0000000000..b95a70d6c1 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h @@ -0,0 +1,31 @@ +/* + * 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 +#include +#include +namespace SimulationInterfaces +{ + //! Result codes to be used in the Result message + //! @see Result.msg + using ErrorCodeType = simulation_interfaces::msg::Result::_result_type; + + //! A message type to represent the result of a failed operation + struct FailedResult + { + FailedResult() = default; + FailedResult(ErrorCodeType errorCode, const AZStd::string& errorString) + : m_errorCode(errorCode) + , m_errorString(errorString) + { + } + 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 new file mode 100644 index 0000000000..08514e8ba3 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -0,0 +1,124 @@ +/* + * 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 "SimulationInterfacesTypeIds.h" + +#include "Result.h" +#include "TagFilter.h" +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + //! A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. + //! @see EntityFilters.msg + struct EntityFilters + { + //! 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_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_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) + }; + + //! @see Spawnable.msg + struct Spawnable + { + AZStd::string m_uri; + AZStd::string m_description; + AZStd::string m_boundsSphere; + }; + + using EntityNameList = AZStd::vector; + using MultipleEntitiesStates = AZStd::unordered_map; + using SpawnableList = AZStd::vector; + using DeletionCompletedCb = AZStd::function&)>; + using SpawnCompletedCb = AZStd::function&)>; + + class SimulationEntityManagerRequests + { + public: + AZ_RTTI(SimulationEntityManagerRequests, SimulationInterfacesRequestsTypeId); + virtual ~SimulationEntityManagerRequests() = default; + + //! # Get a list of entities that match the filter. + //! Supported filters: + //! - name : a posix regular expression to match against entity names + //! - bounds : a shape to use for filtering entities, null means no bounds filtering + //! @see GetEntities.srv + virtual AZ::Outcome GetEntities(const EntityFilters& filter) = 0; + + //! Get the state of an entity. + //! @see GetEntityState.srv + virtual AZ::Outcome GetEntityState(const AZStd::string& name) = 0; + + // clang-format off + //! Get the state of all entities that match the filter. + //! @see GetEntitiesStates.srv + // clang-format on + virtual AZ::Outcome GetEntitiesStates(const EntityFilters& filter) = 0; + + //! Set the state of an entity. + //! @see SetEntityState.srv + virtual AZ::Outcome SetEntityState(const AZStd::string& name, const EntityState& state) = 0; + + //! Remove previously spawned entity from the simulation. + //! @see DeleteEntity.srv + virtual void DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) = 0; + + //! Remove all previously spawned entities from the simulation. + virtual void DeleteAllEntities(DeletionCompletedCb completedCb) = 0; + + //! Get a list of spawnable entities. + //! @see GetSpawnables.srv + virtual AZ::Outcome GetSpawnables() = 0; + + //! Callback for when an entity has been spawned and registered. The string is the name of the entity in the simulation interface. + //! Note: The names are empty, if the entity could not be registered (e.g. prefab has no simulated entities) + virtual void SpawnEntity( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb) = 0; + + //! Reset the simulation to begin. + //! This will revert the entire simulation to the initial state. + virtual void ResetAllEntitiesToInitialState() = 0; + }; + + class SimulationInterfacesBusTraits : public AZ::EBusTraits + { + public: + ////////////////////////////////////////////////////////////////////////// + // EBusTraits overrides + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + ////////////////////////////////////////////////////////////////////////// + }; + + using SimulationEntityManagerRequestBus = AZ::EBus; + using SimulationEntityManagerInterface = AZ::Interface; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h new file mode 100644 index 0000000000..c961a5f3d8 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -0,0 +1,54 @@ +/* + * 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 "SimulationInterfacesTypeIds.h" + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + using SimulationFeatureType = simulation_interfaces::msg::SimulatorFeatures::_features_type::value_type; + + class SimulationFeaturesAggregatorRequests + { + public: + AZ_RTTI(SimulationFeaturesAggregatorRequests, SimulationFeaturesAggregatorRequestsTypeId); + virtual ~SimulationFeaturesAggregatorRequests() = default; + + //! Registers simulation features defined by caller + 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 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(SimulationFeatureType feature) = 0; + }; + + class SimulationFeaturesAggregatorRequestBusTraits : public AZ::EBusTraits + { + public: + ////////////////////////////////////////////////////////////////////////// + // EBusTraits overrides + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + ////////////////////////////////////////////////////////////////////////// + }; + + using SimulationFeaturesAggregatorRequestBus = + AZ::EBus; + using SimulationFeaturesAggregatorRequestBusInterface = AZ::Interface; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h new file mode 100644 index 0000000000..9ab1568ef0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h @@ -0,0 +1,36 @@ +/* + * 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 + +namespace SimulationInterfaces +{ + // System Component TypeIds + inline constexpr const char* SimulationEntitiesManagerTypeId = "{4BF53AF2-A295-4F99-A166-F85FBFBDC077}"; + inline constexpr const char* SimulationEntitiesManagerEditorTypeId = "{B035007B-BAD3-40FA-880F-F45054A4C232}"; + + inline constexpr const char* SimulationManagerTypeId = "{5BB34EB0-1263-4DA1-A35C-CE613A088F4B}"; + inline constexpr const char* SimulationManagerEditorTypeId = "{2CC8D67B-CFD3-4E89-AAF0-8935640B51C1}"; + + inline constexpr const char* SimulationFeaturesAggregatorTypeId = "{5c5ae765-1776-4ba0-8e32-b66c8f4edafe}"; + inline constexpr const char* SimulationFeaturesAggregatorEditorTypeId = "{504A86EF-DF0A-45EC-B69D-315FF4EC8121}"; + + // Module derived classes TypeIds + inline constexpr const char* SimulationInterfacesModuleInterfaceTypeId = "{675797BF-E5D5-438A-BF86-4B4554F09CEF}"; + inline constexpr const char* SimulationInterfacesModuleTypeId = "{8D6741FD-3105-4CB0-9700-152123B6D135}"; + // The Editor Module by default is mutually exclusive with the Client Module + // so they use the Same TypeId + inline constexpr const char* SimulationInterfacesEditorModuleTypeId = SimulationInterfacesModuleTypeId; + + // Interface TypeIds + inline constexpr const char* SimulationInterfacesRequestsTypeId = "{6818E5E3-BBF5-41BD-96BB-7AF57CCC7528}"; + inline constexpr const char* SimulationManagerRequestsTypeId = "{056477BA-8153-4901-9401-0146A5E3E9ED}"; + inline constexpr const char* SimulationFeaturesAggregatorRequestsTypeId = "{099FD08B-B0E2-4705-9C35-CC09C8E45076}"; + inline constexpr const char* SimulationManagerNotificationsTypeId = "{0201067B-9D52-4AB7-9A45-284287F53B00}"; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h new file mode 100644 index 0000000000..deb5c7629c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -0,0 +1,102 @@ +/* + * 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 "SimulationInterfacesTypeIds.h" + +#include "Result.h" +#include +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + using SimulationState = simulation_interfaces::msg::SimulationState::_state_type; + + class SimulationManagerRequests + { + public: + AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); + virtual ~SimulationManagerRequests() = default; + + using ReloadLevelCallback = AZStd::function; + + //! Reload level and remove all spawned simulation entities. + virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0; + + //! Set the simulation to paused or unpaused, + //! expect always to succeed + virtual void SetSimulationPaused(bool paused) = 0; + + //! Step the simulation by a number of steps + //! expect always to succeed + virtual void StepSimulation(AZ::u64 steps) = 0; + + //! Check whether the simulation is paused or not + //! @return boolean value indicating the pause state of the simulation + virtual bool IsSimulationPaused() const = 0; + + //! Cancel executing the simulation steps, if there are remaining steps left + virtual void CancelStepSimulation() = 0; + + //! Check if the SimulationSteps is active + //! @return boolean value indicating if the SimulationSteps is active + virtual bool IsSimulationStepsActive() const = 0; + + //! Get current simulation state + //! @return enum value indicating simulation state based on + //! https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulationState.msg + virtual SimulationState GetSimulationState() const = 0; + + //! Set simulation state + //! @param stateToSet id of the next state to set, based on + //! https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulationState.msg + //! @return outcome indicating if setting state succeed. In case of failure error message with error code is returned + virtual AZ::Outcome SetSimulationState(SimulationState stateToSet) = 0; + }; + + class SimulationMangerRequestBusTraits : public AZ::EBusTraits + { + public: + ////////////////////////////////////////////////////////////////////////// + // EBusTraits overrides + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + ////////////////////////////////////////////////////////////////////////// + }; + + using SimulationManagerRequestBus = AZ::EBus; + using SimulationManagerRequestBusInterface = AZ::Interface; + + class SimulationManagerNotifications + { + public: + AZ_RTTI(SimulationManagerNotifications, SimulationManagerNotificationsTypeId); + virtual ~SimulationManagerNotifications() = default; + + //! Notify about simulation step finish + //! @param remainingSteps - remaining steps to pause simulation + virtual void OnSimulationStepFinish(const AZ::u64 remainingSteps) = 0; + }; + + class SimulationMangerNotificationsBusTraits : public AZ::EBusTraits + { + public: + ////////////////////////////////////////////////////////////////////////// + // EBusTraits overrides + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + ////////////////////////////////////////////////////////////////////////// + }; + + using SimulationManagerNotificationsBus = AZ::EBus; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h new file mode 100644 index 0000000000..f9aa2c40ac --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h @@ -0,0 +1,26 @@ +/* + * 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 +#include + +namespace SimulationInterfaces +{ + //! Structure to design a filter for tags + //! @see TagsFilter.msg + using TagFilterMode = uint8_t; + + struct TagFilter + { + AZStd::unordered_set m_tags; + TagFilterMode m_mode; + }; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Platform/Android/PAL_android.cmake b/Gems/SimulationInterfaces/Code/Platform/Android/PAL_android.cmake new file mode 100644 index 0000000000..0bd6d7cf35 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Android/PAL_android.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_private_files.cmake new file mode 100644 index 0000000000..7fbe5c9178 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_private_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Android +# i.e. ../Source/Android/SimulationInterfacesAndroid.cpp +# ../Source/Android/SimulationInterfacesAndroid.h +# ../Include/Android/SimulationInterfacesAndroid.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_shared_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_shared_files.cmake new file mode 100644 index 0000000000..7fbe5c9178 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_shared_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Android +# i.e. ../Source/Android/SimulationInterfacesAndroid.cpp +# ../Source/Android/SimulationInterfacesAndroid.h +# ../Include/Android/SimulationInterfacesAndroid.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake b/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake new file mode 100644 index 0000000000..238d1b7b24 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED TRUE) diff --git a/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_editor_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_editor_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_editor_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_private_files.cmake new file mode 100644 index 0000000000..e9d0093855 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_private_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Linux +# i.e. ../Source/Linux/SimulationInterfacesLinux.cpp +# ../Source/Linux/SimulationInterfacesLinux.h +# ../Include/Linux/SimulationInterfacesLinux.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_shared_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_shared_files.cmake new file mode 100644 index 0000000000..e9d0093855 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_shared_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Linux +# i.e. ../Source/Linux/SimulationInterfacesLinux.cpp +# ../Source/Linux/SimulationInterfacesLinux.h +# ../Include/Linux/SimulationInterfacesLinux.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Mac/PAL_mac.cmake b/Gems/SimulationInterfaces/Code/Platform/Mac/PAL_mac.cmake new file mode 100644 index 0000000000..0bd6d7cf35 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Mac/PAL_mac.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_editor_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_editor_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_editor_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_private_files.cmake new file mode 100644 index 0000000000..e333821649 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_private_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Mac +# i.e. ../Source/Mac/SimulationInterfacesMac.cpp +# ../Source/Mac/SimulationInterfacesMac.h +# ../Include/Mac/SimulationInterfacesMac.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_shared_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_shared_files.cmake new file mode 100644 index 0000000000..e333821649 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_shared_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Mac +# i.e. ../Source/Mac/SimulationInterfacesMac.cpp +# ../Source/Mac/SimulationInterfacesMac.h +# ../Include/Mac/SimulationInterfacesMac.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Windows/PAL_windows.cmake b/Gems/SimulationInterfaces/Code/Platform/Windows/PAL_windows.cmake new file mode 100644 index 0000000000..0bd6d7cf35 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Windows/PAL_windows.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_editor_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_editor_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_editor_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_private_files.cmake new file mode 100644 index 0000000000..c1fc94516a --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_private_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Windows +# i.e. ../Source/Windows/SimulationInterfacesWindows.cpp +# ../Source/Windows/SimulationInterfacesWindows.h +# ../Include/Windows/SimulationInterfacesWindows.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_shared_files.cmake b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_shared_files.cmake new file mode 100644 index 0000000000..c1fc94516a --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_shared_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for Windows +# i.e. ../Source/Windows/SimulationInterfacesWindows.cpp +# ../Source/Windows/SimulationInterfacesWindows.h +# ../Include/Windows/SimulationInterfacesWindows.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/iOS/PAL_ios.cmake b/Gems/SimulationInterfaces/Code/Platform/iOS/PAL_ios.cmake new file mode 100644 index 0000000000..0bd6d7cf35 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/iOS/PAL_ios.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..17b4532de0 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_api_files.cmake @@ -0,0 +1,8 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_private_files.cmake new file mode 100644 index 0000000000..3c2adb03d2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_private_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for iOS +# i.e. ../Source/iOS/SimulationInterfacesiOS.cpp +# ../Source/iOS/SimulationInterfacesiOS.h +# ../Include/iOS/SimulationInterfacesiOS.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_shared_files.cmake b/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_shared_files.cmake new file mode 100644 index 0000000000..3c2adb03d2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_shared_files.cmake @@ -0,0 +1,13 @@ +# 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 +# + +# Platform specific files for iOS +# i.e. ../Source/iOS/SimulationInterfacesiOS.cpp +# ../Source/iOS/SimulationInterfacesiOS.h +# ../Include/iOS/SimulationInterfacesiOS.h + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h new file mode 100644 index 0000000000..6e6e126863 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h @@ -0,0 +1,126 @@ +/* + * 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 +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + //! Base for each ROS 2 action server handler, forces declaration of features provided by the server + //! combined information along all ROS 2 handlers gives information about simulation features + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures; + template + class ROS2ActionBase : public virtual IROS2HandlerBase + { + public: + using Result = typename RosActionType::Result; + using Feedback = typename RosActionType::Feedback; + using Goal = typename RosActionType::Goal; + using ActionHandle = typename rclcpp_action::Server::SharedPtr; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + virtual ~ROS2ActionBase() = default; + + void Initialize(rclcpp::Node::SharedPtr& node) override + { + CreateAction(node); + } + + protected: + //! This function is called when the action is cancelled + virtual void CancelGoal(std::shared_ptr result) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && m_goalHandle->is_canceling()) + { + m_goalHandle->canceled(result); + m_goalHandle.reset(); + } + } + + //! This function is called when the action successfully finished + virtual void GoalSuccess(std::shared_ptr result) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && (m_goalHandle->is_executing() || m_goalHandle->is_canceling())) + { + m_goalHandle->succeed(result); + m_goalHandle.reset(); + } + } + + //! This function is called on demand, based on the action server implementation, to share the action progress + virtual void PublishFeedback(std::shared_ptr feedback) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && m_goalHandle->is_executing()) + { + m_goalHandle->publish_feedback(feedback); + } + } + + //! This function check if the server is ready to handle new goal + virtual bool IsReadyForExecution() const + { + // Has no goal handle yet - can be accepted. + if (!m_goalHandle) + { + return true; + } + // Accept if the previous one is in a terminal state. + return !(m_goalHandle->is_active() || m_goalHandle->is_executing() || m_goalHandle->is_canceling()); + } + + //! This function is called when the new goal receives + virtual rclcpp_action::GoalResponse GoalReceivedCallback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) = 0; + + //! This function is called when the currently executed goal is cancelled + virtual rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr goal_handle) = 0; + + //! This function is called when the newly received goal is accepted + virtual void GoalAcceptedCallback(const std::shared_ptr goal_handle) = 0; + + //! return features id defined by the handler, ids must follow the definition inside standard: + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + AZStd::unordered_set GetProvidedFeatures() override + { + return {}; + }; + + std::shared_ptr m_goalHandle; + + private: + void CreateAction(rclcpp::Node::SharedPtr& node) + { + // Get the action name from the type name + AZStd::string actionName = RegistryUtilities::GetName(GetTypeName()); + + if (actionName.empty()) + { + // if the action name is empty, use the default name + actionName = GetDefaultName(); + } + + const std::string actionNameStr{ actionName.c_str(), actionName.size() }; + m_actionHandle = rclcpp_action::create_server( + node, + actionNameStr, + AZStd::bind(&ROS2ActionBase::GoalReceivedCallback, this, AZStd::placeholders::_1, AZStd::placeholders::_2), + AZStd::bind(&ROS2ActionBase::GoalCancelledCallback, this, AZStd::placeholders::_1), + AZStd::bind(&ROS2ActionBase::GoalAcceptedCallback, this, AZStd::placeholders::_1)); + } + + ActionHandle m_actionHandle; + }; +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp new file mode 100644 index 0000000000..56caf70182 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "SimulateStepsActionServerHandler.h" + +namespace ROS2SimulationInterfaces +{ + + void SimulateStepsActionServerHandler::Initialize(rclcpp::Node::SharedPtr& node) + { + ROS2ActionBase::Initialize(node); + SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusConnect(); + } + + SimulateStepsActionServerHandler::~SimulateStepsActionServerHandler() + { + AZ::TickBus::Handler::BusDisconnect(); + SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusDisconnect(); + } + + AZStd::unordered_set SimulateStepsActionServerHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_ACTION, + SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE }; + } + + AZStd::string_view SimulateStepsActionServerHandler::GetTypeName() const + { + return "SimulateSteps"; + } + + AZStd::string_view SimulateStepsActionServerHandler::GetDefaultName() const + { + return "simulate_steps"; + } + + rclcpp_action::GoalResponse SimulateStepsActionServerHandler::GoalReceivedCallback( + [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) + { + if (!IsReadyForExecution()) + { + return rclcpp_action::GoalResponse::REJECT; + } + + m_goalSteps = goal->steps; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse SimulateStepsActionServerHandler::GoalCancelledCallback( + [[maybe_unused]] const std::shared_ptr goal_handle) + { + SimulationInterfaces::SimulationManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationManagerRequests::CancelStepSimulation); + m_cancelAction = true; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void SimulateStepsActionServerHandler::GoalAcceptedCallback(const std::shared_ptr goal_handle) + { + m_goalHandle = goal_handle; + + bool isPaused = false; + SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult( + isPaused, &SimulationInterfaces::SimulationManagerRequests::IsSimulationPaused); + if (!isPaused) + { + //! If simulation is not paused then action should not be processed and action server should return Result with result code set + //! to the RESULT_OPERATION_FAILED value, according to the documentation: + //! https://github.com/ros-simulation/simulation_interfaces/blob/main/action/SimulateSteps.action + auto result = std::make_shared(); + result->result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + result->result.error_message = "Request cannot be processed - simulation has to be paused. Action will be aborted."; + m_goalHandle->abort(result); + m_goalHandle.reset(); + return; + } + m_cancelAction = false; + AZ::TickBus::Handler::BusConnect(); + SimulationInterfaces::SimulationManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationManagerRequests::StepSimulation, m_goalSteps); + } + + void SimulateStepsActionServerHandler::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + if (m_cancelAction) + { + auto result = std::make_shared(); + result->result.error_message = "Action has been cancelled."; + result->result.result = simulation_interfaces::msg::Result::RESULT_OK; + CancelGoal(result); + AZ::TickBus::Handler::BusDisconnect(); + return; + } + + // If SimulationSteps is active then it means that SimulationManagerRequestBus::Handler is busy now with a previous request + bool isActive = true; + SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult( + isActive, &SimulationInterfaces::SimulationManagerRequests::IsSimulationStepsActive); + if (!isActive) + { + auto result = std::make_shared(); + result->result.error_message = "Action finished."; + result->result.result = simulation_interfaces::msg::Result::RESULT_OK; + GoalSuccess(result); + AZ::TickBus::Handler::BusDisconnect(); + } + } + + void SimulateStepsActionServerHandler::OnSimulationStepFinish(const AZ::u64 remainingSteps) + { + auto feedback = std::make_shared(); + feedback->remaining_steps = remainingSteps; + feedback->completed_steps = m_goalSteps - remainingSteps; + PublishFeedback(feedback); + } + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h new file mode 100644 index 0000000000..af075c7e4b --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h @@ -0,0 +1,50 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include "ROS2ActionBase.h" +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + + class SimulateStepsActionServerHandler + : public ROS2ActionBase + , public AZ::TickBus::Handler + , public SimulationInterfaces::SimulationManagerNotificationsBus::Handler + { + public: + ~SimulateStepsActionServerHandler(); + + // IROS2HandlerBase overrides + AZStd::unordered_set GetProvidedFeatures() override; + AZStd::string_view GetTypeName() const override; + AZStd::string_view GetDefaultName() const override; + void Initialize(rclcpp::Node::SharedPtr& node) override; + + // ROS2ActionBase overrides + rclcpp_action::GoalResponse GoalReceivedCallback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) override; + rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr goal_handle) override; + void GoalAcceptedCallback(const std::shared_ptr goal_handle) override; + + protected: + // AZ::TickBus::Handler overrides + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + // SimulationInterfaces::SimulationManagerNotificationsBus::Handler + void OnSimulationStepFinish(const AZ::u64 remainingSteps) override; + + private: + AZ::u64 m_goalSteps{ 0 }; + bool m_cancelAction{ false }; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp new file mode 100644 index 0000000000..dfbe95d037 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp @@ -0,0 +1,31 @@ +/* + * 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 "CommonUtilities.h" + +namespace SimulationInterfaces::Utils +{ + const char* const ProductAssetPrefix = "product_asset:///"; + AZStd::string RelPathToUri(AZStd::string_view relPath) + { + AZStd::string uri = relPath; + AZStd::replace(uri.begin(), uri.end(), '\\', '/'); + uri.insert(0, ProductAssetPrefix); + return uri; + } + + AZStd::string UriToRelPath(AZStd::string_view uri) + { + if (uri.starts_with(ProductAssetPrefix)) + { + const AZStd::string_view productAssetPrefix{ ProductAssetPrefix }; + return uri.substr(productAssetPrefix.length()); + } + return {}; + } +} // namespace SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h new file mode 100644 index 0000000000..2ca5f2c4ca --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h @@ -0,0 +1,20 @@ +/* + * 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 +namespace SimulationInterfaces::Utils +{ + //! Convert a relative path to a URI + //! relative path: "path/to/file.txt" + //! URI: "product_asset:///path/to/file.txt" + AZStd::string RelPathToUri(AZStd::string_view relPath); + AZStd::string UriToRelPath(AZStd::string_view relPath); + +} // namespace SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp new file mode 100644 index 0000000000..ad31a1e9e3 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp @@ -0,0 +1,112 @@ +/* + * 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 "ROS2SimulationInterfacesSystemComponent.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + AZ_COMPONENT_IMPL( + ROS2SimulationInterfacesSystemComponent, "ROS2SimulationInterfacesSystemComponent", ROS2SimulationInterfacesSystemComponentTypeId); + + void ROS2SimulationInterfacesSystemComponent::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + void ROS2SimulationInterfacesSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("ROS2SimulationInterfacesService")); + } + + void ROS2SimulationInterfacesSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("ROS2SimulationInterfacesService")); + } + + void ROS2SimulationInterfacesSystemComponent::GetRequiredServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("ROS2Service")); + } + + void ROS2SimulationInterfacesSystemComponent::GetDependentServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + } + + void ROS2SimulationInterfacesSystemComponent::Activate() + { + ROS2SimulationInterfacesRequestBus::Handler::BusConnect(); + + rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); + AZ_Assert(ros2Node, "ROS2 node is not available."); + + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + RegisterInterface(ros2Node); + } + + void ROS2SimulationInterfacesSystemComponent::Deactivate() + { + ROS2SimulationInterfacesRequestBus::Handler::BusDisconnect(); + + for (auto& [handlerType, handler] : m_availableRos2Interface) + { + handler.reset(); + } + } + + AZStd::unordered_set ROS2SimulationInterfacesSystemComponent::GetSimulationFeatures() + { + AZStd::unordered_set result; + for (auto& [handlerType, handler] : m_availableRos2Interface) + { + auto features = handler->GetProvidedFeatures(); + result.insert(features.begin(), features.end()); + } + return result; + } + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h new file mode 100644 index 0000000000..199ca7d163 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h @@ -0,0 +1,62 @@ +/* + * 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 +#include +#include +#include +#include +#include + +#include + +#include + +namespace ROS2SimulationInterfaces +{ + class ROS2SimulationInterfacesSystemComponent + : public AZ::Component + , public ROS2SimulationInterfacesRequestBus::Handler + { + public: + AZ_COMPONENT_DECL(ROS2SimulationInterfacesSystemComponent); + + static void Reflect(AZ::ReflectContext* context); + + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + ROS2SimulationInterfacesSystemComponent() = default; + ~ROS2SimulationInterfacesSystemComponent() = default; + + protected: + // AZ::Component interface implementation + void Activate() override; + void Deactivate() override; + + // ROS2SimulationInterfacesRequestBus override + AZStd::unordered_set GetSimulationFeatures() override; + + private: + AZStd::unordered_map> m_availableRos2Interface; + + template + void RegisterInterface(rclcpp::Node::SharedPtr ros2Node) + { + AZStd::shared_ptr handler = AZStd::make_shared(); + handler->Initialize(ros2Node); + m_availableRos2Interface[handler->GetTypeName()] = AZStd::move(handler); + handler.reset(); + }; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp new file mode 100644 index 0000000000..9b48df0c59 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -0,0 +1,783 @@ +/* + * 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 "SimulationEntitiesManager.h" + +#include +#include +#include + +#include "CommonUtilities.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + namespace + { + void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) + { + 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); + } + } + + 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; + } + + } // namespace + + AZ_COMPONENT_IMPL(SimulationEntitiesManager, "SimulationEntitiesManager", SimulationEntitiesManagerTypeId); + + void SimulationEntitiesManager::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + void SimulationEntitiesManager::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("SimulationInterfacesService")); + } + + void SimulationEntitiesManager::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("SimulationInterfacesService")); + } + + void SimulationEntitiesManager::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("AssetCatalogService")); + required.push_back(AZ_CRC_CE("SimulationFeaturesAggregator")); + } + + void SimulationEntitiesManager::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + dependent.push_back(AZ_CRC_CE("PhysicsService")); + dependent.push_back(AZ_CRC_CE("SimulationFeaturesAggregator")); + } + + SimulationEntitiesManager::SimulationEntitiesManager() + { + if (SimulationEntityManagerInterface::Get() == nullptr) + { + SimulationEntityManagerInterface::Register(this); + } + } + + SimulationEntitiesManager::~SimulationEntitiesManager() + { + if (SimulationEntityManagerInterface::Get() == this) + { + SimulationEntityManagerInterface::Unregister(this); + } + } + + bool SimulationEntitiesManager::RegisterNewSimulatedBody(AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) + { + auto* scene = GetSceneHelper(sceneHandle); + AZ_Assert(scene, "Scene is not available."); + if (scene == nullptr) + { + return false; + } + auto* body = scene->GetSimulatedBodyFromHandle(bodyHandle); + if (body == nullptr) + { + AZ_Trace("SimulationInterfaces", "Simulated body pointer is not valid"); + return false; + } + auto* rigidBody = azdynamic_cast(body); + if (rigidBody != nullptr) + { + [[maybe_unused]] auto shapeCount = rigidBody->GetShapeCount(); + AZ_Warning( + "SimulationInterfaces", + shapeCount > 0, + "Entity %s has no collider shapes, it won't be available by bound search", + rigidBody->GetEntityId().ToString().c_str()); + } + const AZ::EntityId entityId = body->GetEntityId(); + if (!entityId.IsValid()) + { + AZ_Trace("SimulationInterfaces", "EntityId is not valid"); + return false; + } + AZ::Entity* entity = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId); + if (entity == nullptr) + { + AZ_Trace("SimulationInterfaces", "Entity pointer is not valid"); + return false; + } + // check if entity is not spawned by this component + const auto ticketId = entity->GetEntitySpawnTicketId(); + + auto spawnDataIt = m_spawnCompletedCallbacks.find(ticketId); + const bool wasSpawned = spawnDataIt != m_spawnCompletedCallbacks.end(); + + const AZStd::string proposedName = wasSpawned ? spawnDataIt->second.m_userProposedName : entity->GetName(); + + // register entity + const AZStd::string registeredName = this->AddSimulatedEntity(entityId, proposedName); + + // cache registered name for later use in SpawnCompletionCallback + if (wasSpawned && !spawnDataIt->second.m_registered) + { + spawnDataIt->second.m_registered = true; + spawnDataIt->second.m_resultedName = registeredName; + } + + // cache the initial state - for simulator reset with SCOPE_STATE. + EntityState initialState{}; + initialState.m_pose = entity->GetTransform()->GetWorldTM(); + if (rigidBody) + { + initialState.m_twistLinear = rigidBody->GetLinearVelocity(); + initialState.m_twistAngular = rigidBody->GetAngularVelocity(); + } + m_entityIdToInitialState[entityId] = initialState; + AZ_Info("SimulationInterfaces", "Registered entity %s\n", registeredName.c_str()); + return true; + } + + AZStd::vector> SimulationEntitiesManager:: + RegisterNewSimulatedBodies(const AZStd::vector>& handles) + { + AZStd::vector> unconfiguredHandles; + for (const auto& handle : handles) + { + if (!RegisterNewSimulatedBody(handle.first, handle.second)) + { + unconfiguredHandles.push_back(handle); + } + } + return unconfiguredHandles; + } + + void SimulationEntitiesManager::Activate() + { + m_simulationBodyAddedHandler = AzPhysics::SceneEvents::OnSimulationBodyAdded::Handler( + [this](AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) + { + m_unconfiguredScenesHandles.push_back(AZStd::make_pair(sceneHandle, bodyHandle)); + m_unconfiguredScenesHandles = RegisterNewSimulatedBodies(m_unconfiguredScenesHandles); + }); + m_simulationBodyRemovedHandler = AzPhysics::SceneEvents::OnSimulationBodyRemoved::Handler( + [this](AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) + { + auto* scene = GetSceneHelper(sceneHandle); + if (scene == nullptr) + { + return; + } + const auto* body = scene->GetSimulatedBodyFromHandle(bodyHandle); + AZ_Assert(body, "Simulated body is not available."); + const AZ::EntityId entityId = body->GetEntityId(); + // remove simulated entity + this->RemoveSimulatedEntity(entityId); + }); + + m_sceneAddedHandler = AzPhysics::SystemEvents::OnSceneAddedEvent::Handler( + [this](AzPhysics::SceneHandle sceneHandle) + { + 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) + { + return; + } + scene->RegisterSimulationBodyAddedHandler(m_simulationBodyAddedHandler); + scene->RegisterSimulationBodyRemovedHandler(m_simulationBodyRemovedHandler); + + AZ_Printf("SimulationInterfaces", "Registered simulation body added handler\n"); + m_physicsScenesHandle = sceneHandle; + }); + m_sceneRemovedHandler = AzPhysics::SystemEvents::OnSceneRemovedEvent::Handler( + [this](AzPhysics::SceneHandle sceneHandle) + { + if (m_physicsScenesHandle == sceneHandle) + { + m_entityIdToSimulatedEntityMap.clear(); + m_simulatedEntityToEntityIdMap.clear(); + m_simulationBodyAddedHandler.Disconnect(); + m_simulationBodyRemovedHandler.Disconnect(); + m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; + } + }); + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + if (physicsSystem) + { + physicsSystem->RegisterSceneAddedEvent(m_sceneAddedHandler); + physicsSystem->RegisterSceneRemovedEvent(m_sceneRemovedHandler); + SimulationEntityManagerRequestBus::Handler::BusConnect(); + } + + SimulationFeaturesAggregatorRequestBus::Broadcast( + &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, + AZStd::unordered_set{ + // not implemented: simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, + // not implemented: simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, + // not implemented: 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(); + } + + void SimulationEntitiesManager::Deactivate() + { + AZ::TickBus::Handler::BusDisconnect(); + SimulationEntityManagerRequestBus::Handler::BusDisconnect(); + + m_simulationBodyAddedHandler.Disconnect(); + m_simulationBodyRemovedHandler.Disconnect(); + + m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; + + m_sceneAddedHandler.Disconnect(); + } + + AZStd::string SimulationEntitiesManager::AddSimulatedEntity(AZ::EntityId entityId, const AZStd::string& userProposedName) + { + if (!entityId.IsValid()) + { + return ""; + } + // check if entity is already registered + auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); + if (findIt != m_entityIdToSimulatedEntityMap.end()) + { + return findIt->second; + } + // register entity under unique name + AZStd::string simulatedEntityName = GetSimulatedEntityName(entityId, userProposedName); + m_simulatedEntityToEntityIdMap[simulatedEntityName] = entityId; + m_entityIdToSimulatedEntityMap[entityId] = simulatedEntityName; + return simulatedEntityName; + } + + void SimulationEntitiesManager::RemoveSimulatedEntity(AZ::EntityId entityId) + { + if (auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); findIt != m_entityIdToSimulatedEntityMap.end()) + { + const auto& simulatedEntityName = findIt->second; + m_entityIdToSimulatedEntityMap.erase(findIt); + m_simulatedEntityToEntityIdMap.erase(simulatedEntityName); + } + if (auto findIt = m_entityIdToInitialState.find(entityId); findIt != m_entityIdToInitialState.end()) + { + m_entityIdToInitialState.erase(findIt); + } + } + + AZ::Outcome SimulationEntitiesManager::GetEntities(const EntityFilters& filter) + { + 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_nameFilter.empty(); + const bool shapeCastFilter = filter.m_boundsShape != nullptr; + + AZStd::vector entities; + if (!shapeCastFilter) + { + // get all entities from the map + entities.reserve(m_entityIdToSimulatedEntityMap.size()); + AZStd::transform( + m_entityIdToSimulatedEntityMap.begin(), + m_entityIdToSimulatedEntityMap.end(), + AZStd::back_inserter(entities), + [](const auto& pair) + { + return pair.second; + }); + } + else + { + auto* sceneInterface = AZ::Interface::Get(); + AZ_Assert(sceneInterface, "Physics scene interface is not available."); + + if (m_physicsScenesHandle == AzPhysics::InvalidSceneHandle) + { + return AZ::Failure( + FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, "Physics scene interface is not available.")); + } + + AzPhysics::OverlapRequest request; + 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); + for (const auto& hit : result.m_hits) + { + const AZ::EntityId entityId = hit.m_entityId; + if (auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); findIt != m_entityIdToSimulatedEntityMap.end()) + { + entities.push_back(findIt->second); + } + } + } + if (reFilter) + { + const AZStd::vector prefilteredEntities = AZStd::move(entities); + entities.clear(); + const AZStd::regex regex(filter.m_nameFilter, AZStd::regex::extended); + if (!regex.Valid()) + { + AZ_Warning("SimulationInterfaces", false, "Invalid regex filter"); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Invalid regex filter")); + } + AZStd::ranges::copy_if( + prefilteredEntities, + AZStd::back_inserter(entities), + [®ex](const AZStd::string& entityName) + { + return AZStd::regex_search(entityName, regex); + }); + } + return AZ::Success(entities); + } + + AZ::Outcome SimulationEntitiesManager::GetEntityState(const AZStd::string& name) + { + const auto findIt = m_simulatedEntityToEntityIdMap.find(name); + if (findIt == m_simulatedEntityToEntityIdMap.end()) + { + AZ_Warning("SimulationInterfaces", false, "Entity %s not found", name.c_str()); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found")); + } + EntityState entityState{}; + const AZ::EntityId entityId = findIt->second; + AZ_Assert(entityId.IsValid(), "EntityId is not valid"); + + AZ::TransformBus::EventResult(entityState.m_pose, entityId, &AZ::TransformBus::Events::GetWorldTM); + + AZ::Vector3 linearVelocity = AZ::Vector3::CreateZero(); + Physics::RigidBodyRequestBus::EventResult(linearVelocity, entityId, &Physics::RigidBodyRequests::GetLinearVelocity); + + AZ::Vector3 angularVelocity = AZ::Vector3::CreateZero(); + Physics::RigidBodyRequestBus::EventResult(angularVelocity, entityId, &Physics::RigidBodyRequests::GetAngularVelocity); + + // transform linear and angular velocities to entity frame + const AZ::Transform entityTransformInv = entityState.m_pose.GetInverse(); + 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_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")); + } + MultipleEntitiesStates entitiesStates; + const auto& entities = GetEntities(filter); + if (!entities.IsSuccess()) + { + return AZ::Failure(entities.GetError()); + } + for (const auto& entity : entities.GetValue()) + { + auto state = GetEntityState(entity); + if (!state.IsSuccess()) + { + return AZ::Failure(state.GetError()); + } + entitiesStates.emplace(AZStd::make_pair(entity, state.GetValue())); + } + return entitiesStates; + } + + AZ::Outcome SimulationEntitiesManager::SetEntityState(const AZStd::string& name, const EntityState& state) + { + const auto findIt = m_simulatedEntityToEntityIdMap.find(name); + if (findIt == m_simulatedEntityToEntityIdMap.end()) + { + AZ_Warning("SimulationInterfaces", false, "Entity %s not found", name.c_str()); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found")); + } + + const AZ::EntityId entityId = findIt->second; + AZ_Assert(entityId.IsValid(), "EntityId is not valid"); + + // get entity and all descendants + AZStd::vector entityAndDescendants; + AZ::TransformBus::EventResult(entityAndDescendants, entityId, &AZ::TransformBus::Events::GetEntityAndAllDescendants); + SetEntitiesState(entityAndDescendants, state); + return AZ::Success(); + } + + void SimulationEntitiesManager::SetEntitiesState(const AZStd::vector& entityAndDescendants, const EntityState& state) + { + if (entityAndDescendants.empty()) + { + AZ_Error("SimulationInterfaces", false, "Entity and descendants list is empty"); + 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 + for (const auto& descendant : entityAndDescendants) + { + AzPhysics::SimulatedBodyComponentRequestsBus::Event(descendant, &AzPhysics::SimulatedBodyComponentRequests::DisablePhysics); + } + 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) + { + AzPhysics::SimulatedBodyComponentRequestsBus::Event(descendant, &AzPhysics::SimulatedBodyComponentRequests::EnablePhysics); + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::SetAngularVelocity, AZ::Vector3::CreateZero()); + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::SetLinearVelocity, AZ::Vector3::CreateZero()); + } + } + + if (!state.m_twistLinear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twistAngular.IsZero(AZ::Constants::FloatEpsilon)) + { + // get rigid body + AzPhysics::RigidBody* rigidBody = nullptr; + Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody); + if (rigidBody != nullptr) + { + SetRigidBodyVelocities(rigidBody, state); + } + } + } + + void SimulationEntitiesManager::DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) + { + const auto findIt = m_simulatedEntityToEntityIdMap.find(name); + if (findIt == m_simulatedEntityToEntityIdMap.end()) + { + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found"))); + return; + } + + const AZ::EntityId entityId = findIt->second; + AZ_Assert(entityId.IsValid(), "EntityId is not valid"); + // get entity + AZ::Entity* entity = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId); + AZ_Assert(entity, "Entity is not available."); + if (entity == nullptr) + { + AZ_Error("SimulationInterfaces", false, "Entity %s (%s) not found", name.c_str(), entityId.ToString().c_str()); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found"))); + return; + } + // check if entity is spawned by this component + const auto ticketId = entity->GetEntitySpawnTicketId(); + if (m_spawnedTickets.find(ticketId) != m_spawnedTickets.end()) + { + // get spawner + auto spawner = AZ::Interface::Get(); + AZ_Assert(spawner, "SpawnableEntitiesDefinition is not available."); + // get ticket + auto ticket = m_spawnedTickets[ticketId]; + // remove ticket + AzFramework::DespawnAllEntitiesOptionalArgs optionalArgs; + optionalArgs.m_completionCallback = [this, completedCb](AzFramework::EntitySpawnTicket::Id ticketId) + { + m_spawnedTickets.erase(ticketId); + completedCb(AZ::Success()); + }; + spawner->DespawnAllEntities(ticket, optionalArgs); + } + else + { + 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))); + } + } + + 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::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 SimulationEntitiesManager::GetSpawnables() + { + AZStd::vector spawnables; + + const auto enumCallback = [&spawnables](const AZ::Data::AssetId assetId, const AZ::Data::AssetInfo& assetInfo) + { + bool isSpawnable = false; + AZ::Data::AssetCatalogRequestBus::BroadcastResult( + isSpawnable, &AZ::Data::AssetCatalogRequests::DoesAssetIdMatchWildcardPattern, assetId, "*.spawnable"); + + if (isSpawnable) + { + Spawnable spawnable; + spawnable.m_uri = Utils::RelPathToUri(assetInfo.m_relativePath); + spawnables.push_back(spawnable); + } + }; + + AZ::Data::AssetCatalogRequestBus::Broadcast(&AZ::Data::AssetCatalogRequests::EnumerateAssets, nullptr, enumCallback, nullptr); + return AZ::Success(spawnables); + } + + void SimulationEntitiesManager::SpawnEntity( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb) + { + if (!allowRename) + { + // If API user does not allow renaming, check if name is unique + if (m_simulatedEntityToEntityIdMap.contains(name)) + { + const auto msg = AZStd::string::format("Entity name %s is not unique", name.c_str()); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::srv::SpawnEntity::Response::NAME_NOT_UNIQUE, msg))); + return; + } + if (name.empty()) + { + const auto msg = AZStd::string::format("Entity name is empty"); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID, msg))); + return; + } + } + if (!initialPose.IsOrthogonal()) + { + AZ_Warning("SimulationInterfaces", false, "Initial pose is not orthogonal"); + completedCb(AZ::Failure(FailedResult( + simulation_interfaces::srv::SpawnEntity::Response::INVALID_POSE, "Initial pose is not orthogonal"))); // INVALID_POSE + return; + } + + // get rel path from uri + const AZStd::string relPath = Utils::UriToRelPath(uri); + + // create spawnable + AZ::Data::AssetId assetId; + AZ::Data::AssetCatalogRequestBus::BroadcastResult( + assetId, + &AZ::Data::AssetCatalogRequestBus::Events::GetAssetIdByPath, + relPath.c_str(), + azrtti_typeid(), + false); + AZ_Warning("SimulationInterfaces", assetId.IsValid(), "AssetId is not valid, relative path %s", relPath.c_str()); + + auto spawner = AZ::Interface::Get(); + AZ_Assert(spawner, "SpawnableEntitiesDefinition is not available."); + + AZ::Data::Asset spawnableAsset = + AZ::Data::AssetManager::Instance().GetAsset(assetId, AZ::Data::AssetLoadBehavior::NoLoad); + if (!spawnableAsset) + { + const auto msg = AZStd::string::format("Spawnable asset %s not found", uri.c_str()); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, msg))); + return; + } + + auto ticket = AzFramework::EntitySpawnTicket(spawnableAsset); + AzFramework::SpawnAllEntitiesOptionalArgs optionalArgs; + + optionalArgs.m_preInsertionCallback = [initialPose, entityNamespace, name](auto id, auto view) + { + if (view.empty()) + { + return; + } + + for (auto* entity : view) + { + ROS2::ROS2FrameComponent* frameComponent = entity->template FindComponent(); + if (frameComponent) + { + const AZStd::string f = frameComponent->GetFrameID(); + if (f.empty()) + { + frameComponent->SetFrameID(name); + } + else + { + frameComponent->SetFrameID(AZStd::string::format("%s/%s", entityNamespace.c_str(), f.c_str())); + } + } + } + const AZ::Entity* root = *view.begin(); + auto* transformInterface = root->FindComponent(); + if (transformInterface) + { + transformInterface->SetWorldTM(initialPose); + } + }; + optionalArgs.m_completionCallback = + [this, uri](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) + { + m_unconfiguredScenesHandles = RegisterNewSimulatedBodies(m_unconfiguredScenesHandles); + + // at this point the entities are spawned and should be registered in simulation interface and callback should be called + // if that is not a case, it means that the AZFramework::Physics::OnSimulationBodyAdded event was not called. + // That means the prefab has no physics component or the physics component is not enabled - we need to call the callback here + // and return the error. + auto spawnData = m_spawnCompletedCallbacks.find(ticketId); + if (spawnData != m_spawnCompletedCallbacks.end()) + { + // call the API user's callback, when the entity was registered + if (spawnData->second.m_registered) + { + spawnData->second.m_completedCb(AZ::Success(spawnData->second.m_resultedName)); + } + else + { + // call the error callback, when the entity was not registered + const auto msg = AZStd::string::format( + "Entity %s (uri : %s) was not registered in simulation interface - " + "no physics component or physics component is not enabled in source prefab.\n" + "Entity will be in simulation, but not available in simulation interface.\n" + "Please add some physics component (at least one static rigid body component) to the prefab.\n" + "Technically, it is a memory leak.\n", + spawnData->second.m_userProposedName.c_str(), + uri.c_str()); + spawnData->second.m_completedCb(msg); + } + m_spawnCompletedCallbacks.erase(spawnData); + } + }; + + spawner->SpawnAllEntities(ticket, optionalArgs); + auto ticketId = ticket.GetId(); + AZ_Info("SimulationInterfaces", "Spawning uri %s with ticket id %d\n", uri.c_str(), ticketId); + + SpawnCompletedCbData data; + data.m_userProposedName = name; + data.m_completedCb = completedCb; + m_spawnCompletedCallbacks[ticketId] = data; + m_spawnedTickets[ticketId] = ticket; + } + + AZStd::string SimulationEntitiesManager::GetSimulatedEntityName(AZ::EntityId entityId, const AZStd::string& proposedName) const + { + // Get O3DE entity name + AZStd::string newName = proposedName; + // check if name is not unique. If not, add Entity Name to name + if (m_simulatedEntityToEntityIdMap.contains(newName)) + { + AZStd::string entityName; + AZ::ComponentApplicationBus::BroadcastResult(entityName, &AZ::ComponentApplicationRequests::GetEntityName, entityId); + // name is not unique, add entityId to name + newName = AZStd::string::format("%s_%s", newName.c_str(), entityName.c_str()); + } + + // check if name is still not unique, if not, add EntityId to name + if (m_simulatedEntityToEntityIdMap.contains(newName)) + { + newName = AZStd::string::format("%s_%s", newName.c_str(), entityId.ToString().c_str()); + } + return newName; + } + + void SimulationEntitiesManager::ResetAllEntitiesToInitialState() + { + for (const auto& [entityId, initialState] : m_entityIdToInitialState) + { + AZStd::vector entityAndDescendants; + AZ::TransformBus::EventResult(entityAndDescendants, entityId, &AZ::TransformBus::Events::GetEntityAndAllDescendants); + + SetEntitiesState(entityAndDescendants, initialState); + } + } + + void SimulationEntitiesManager::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + m_unconfiguredScenesHandles = RegisterNewSimulatedBodies(m_unconfiguredScenesHandles); + } +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h new file mode 100644 index 0000000000..215845df66 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -0,0 +1,118 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + class SimulationEntitiesManager + : public AZ::Component + , protected SimulationEntityManagerRequestBus::Handler + , protected AZ::TickBus::Handler + { + public: + AZ_COMPONENT_DECL(SimulationEntitiesManager); + + static void Reflect(AZ::ReflectContext* context); + + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + SimulationEntitiesManager(); + ~SimulationEntitiesManager(); + // AZ::Component interface implementation + void Activate() override; + void Deactivate() override; + + private: + // SimulationEntityManagerRequestBus interface implementation + AZ::Outcome GetEntities(const EntityFilters& filter) override; + AZ::Outcome GetEntityState(const AZStd::string& name) override; + AZ::Outcome GetEntitiesStates(const EntityFilters& filter) override; + AZ::Outcome 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 GetSpawnables() override; + void SpawnEntity( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb) override; + void ResetAllEntitiesToInitialState() override; + + // AZ::TickBus::Handler interface implementation + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + //! 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 + //! \param sceneHandle The scene handle to register the body to + //! \param bodyHandle The body handle to register + bool RegisterNewSimulatedBody(AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle); + + //! 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); + + //! Registers simulated entity to entity id mapping. + //! Note that the entityId will be registered under unique name. + //! \param entityId The entity id to register + //! \param proposedName Optional user proposed name for the simulated entity + //! \return returns the simulated entity name + AZStd::string AddSimulatedEntity(AZ::EntityId entityId, const AZStd::string& proposedName); + + //! Removes simulated entity from the mapping. + void RemoveSimulatedEntity(AZ::EntityId entityId); + + //! Returns the simulated entity name for the given entity id. + AZStd::string GetSimulatedEntityName(AZ::EntityId entityId, const AZStd::string& proposedName) const; + + //! 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; + + AzPhysics::SystemEvents::OnSceneAddedEvent::Handler m_sceneAddedHandler; + 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::unordered_map m_simulatedEntityToEntityIdMap; + AZStd::unordered_map m_entityIdToSimulatedEntityMap; + AZStd::unordered_map m_entityIdToInitialState; + + AZStd::unordered_map m_spawnedTickets; + + struct SpawnCompletedCbData + { + AZStd::string m_userProposedName; //! Name proposed by the User in spawn request + AZStd::string m_resultedName; //! Name of the entity in the simulation interface + SpawnCompletedCb m_completedCb; //! User callback to be called when the entity is registered + AZ::ScriptTimePoint m_spawnCompletedTime; //! Time at which the entity was spawned + bool m_registered = false; //! Flag to check if the entity was registered + }; + AZStd::unordered_map + m_spawnCompletedCallbacks; //! Callbacks to be called when the entity is registered + }; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp new file mode 100644 index 0000000000..b7250f034c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp @@ -0,0 +1,88 @@ +/* + * 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 "SimulationFeaturesAggregator.h" + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + + AZ_COMPONENT_IMPL(SimulationFeaturesAggregator, "SimulationFeaturesAggregator", SimulationFeaturesAggregatorTypeId); + + void SimulationFeaturesAggregator::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + void SimulationFeaturesAggregator::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("SimulationFeaturesAggregator")); + } + + void SimulationFeaturesAggregator::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("SimulationFeaturesAggregator")); + } + + void SimulationFeaturesAggregator::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + } + + void SimulationFeaturesAggregator::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + } + + SimulationFeaturesAggregator::SimulationFeaturesAggregator() + { + if (SimulationFeaturesAggregatorRequestBusInterface::Get() == nullptr) + { + SimulationFeaturesAggregatorRequestBusInterface::Register(this); + } + } + + SimulationFeaturesAggregator::~SimulationFeaturesAggregator() + { + if (SimulationFeaturesAggregatorRequestBusInterface::Get() == this) + { + SimulationFeaturesAggregatorRequestBusInterface::Unregister(this); + } + } + + void SimulationFeaturesAggregator::Activate() + { + SimulationFeaturesAggregatorRequestBus::Handler::BusConnect(); + } + + void SimulationFeaturesAggregator::Deactivate() + { + SimulationFeaturesAggregatorRequestBus::Handler::BusDisconnect(); + } + + void SimulationFeaturesAggregator::AddSimulationFeatures(const AZStd::unordered_set& features) + { + m_registeredFeatures.insert(features.begin(), features.end()); + } + + AZStd::unordered_set SimulationFeaturesAggregator::GetSimulationFeatures() + { + return m_registeredFeatures; + } + + bool SimulationFeaturesAggregator::HasFeature(SimulationFeatureType feature) + { + return m_registeredFeatures.contains(feature); + } + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h new file mode 100644 index 0000000000..45cc269eda --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h @@ -0,0 +1,50 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + class SimulationFeaturesAggregator + : public AZ::Component + , protected SimulationFeaturesAggregatorRequestBus::Handler + { + public: + AZ_COMPONENT_DECL(SimulationFeaturesAggregator); + + static void Reflect(AZ::ReflectContext* context); + + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + SimulationFeaturesAggregator(); + ~SimulationFeaturesAggregator(); + + // AZ::Component + void Activate() override; + void Deactivate() override; + + private: + // SimulationFeaturesAggregatorRequestBus overrides + void AddSimulationFeatures(const AZStd::unordered_set& features) override; + AZStd::unordered_set GetSimulationFeatures() override; + bool HasFeature(SimulationFeatureType feature) override; + + AZStd::unordered_set m_registeredFeatures; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp new file mode 100644 index 0000000000..a1dc3ba89e --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp @@ -0,0 +1,23 @@ +/* + * 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 "SimulationEntitiesManager.h" +#include +#include + +namespace SimulationInterfaces +{ + class SimulationInterfacesModule : public SimulationInterfacesModuleInterface + { + public: + AZ_RTTI(SimulationInterfacesModule, SimulationInterfacesModuleTypeId, SimulationInterfacesModuleInterface); + AZ_CLASS_ALLOCATOR(SimulationInterfacesModule, AZ::SystemAllocator); + }; +} // namespace SimulationInterfaces + +AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfaces, SimulationInterfaces::SimulationInterfacesModule) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp new file mode 100644 index 0000000000..108b2bf099 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -0,0 +1,383 @@ +/* + * 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 "SimulationManager.h" +#include "SimulationInterfaces/SimulationMangerRequestBus.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + namespace + { + + const AZStd::unordered_map SimulationStateToString = { + { simulation_interfaces::msg::SimulationState::STATE_PAUSED, "STATE_PAUSED" }, + { simulation_interfaces::msg::SimulationState::STATE_PLAYING, "STATE_PLAYING" }, + { simulation_interfaces::msg::SimulationState::STATE_QUITTING, "STATE_QUITTING" }, + { simulation_interfaces::msg::SimulationState::STATE_STOPPED, "STATE_STOPPED" } + }; + + constexpr AZStd::string_view PrintStateName = "/SimulationInterfaces/PrintStateNameInGui"; + constexpr AZStd::string_view StartInStoppedStateKey = "/SimulationInterfaces/StartInStoppedState"; + + AZStd::string GetStateName(SimulationState state) + { + auto it = SimulationStateToString.find(state); + if (it != SimulationStateToString.end()) + { + return it->second; + } + return AZStd::string::format("Unknown state: %d", aznumeric_cast(state)); + } + + bool StartInStoppedState() + { + AZ::SettingsRegistryInterface* settingsRegistry = AZ::SettingsRegistry::Get(); + AZ_Assert(settingsRegistry, "Settings Registry is not available"); + bool output = true; + settingsRegistry->Get(output, StartInStoppedStateKey); + return output; + } + + bool PrintStateNameInGui() + { + AZ::SettingsRegistryInterface* settingsRegistry = AZ::SettingsRegistry::Get(); + AZ_Assert(settingsRegistry, "Settings Registry is not available"); + bool output = true; + settingsRegistry->Get(output, PrintStateName); + return output; + } + } // namespace + + AZ_COMPONENT_IMPL(SimulationManager, "SimulationManager", SimulationManagerTypeId); + + void SimulationManager::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + void SimulationManager::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("SimulationManagerService")); + } + + void SimulationManager::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("SimulationManagerService")); + } + + void SimulationManager::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("PhysicsService")); + required.push_back(AZ_CRC_CE("SimulationFeaturesAggregator")); + } + + void SimulationManager::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + dependent.push_back(AZ_CRC_CE("SimulationFeaturesAggregator")); + dependent.push_back(AZ_CRC_CE("DebugDrawTextService")); + } + + SimulationManager::SimulationManager() + { + if (SimulationManagerRequestBusInterface::Get() == nullptr) + { + SimulationManagerRequestBusInterface::Register(this); + } + } + + SimulationManager::~SimulationManager() + { + if (SimulationManagerRequestBusInterface::Get() == this) + { + SimulationManagerRequestBusInterface::Unregister(this); + } + } + + void SimulationManager::InitializeSimulationState() + { + // if start in stopped state, pause simulation. Default state for simulation by the standard is STOPPED and + // SetSimulationState has logic to prevent transition to the same state. + if (StartInStoppedState()) + { + m_simulationState = simulation_interfaces::msg::SimulationState::STATE_STOPPED; + SetSimulationPaused(true); + } + else + { + SetSimulationState(simulation_interfaces::msg::SimulationState::STATE_PLAYING); + } + } + + void SimulationManager::Activate() + { + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); + 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 }); + if (PrintStateNameInGui()) + { + AZ::TickBus::Handler::BusConnect(); + } + AZ::SystemTickBus::QueueFunction( + [this]() + { + InitializeSimulationState(); + }); + } + + void SimulationManager::Deactivate() + { + AZ::TickBus::Handler::BusDisconnect(); + SimulationManagerRequestBus::Handler::BusDisconnect(); + } + + bool SimulationManager::IsSimulationPaused() const + { + return m_isSimulationPaused; + } + + bool SimulationManager::IsSimulationStepsActive() const + { + return m_simulationFinishEvent.IsConnected(); + } + + void SimulationManager::CancelStepSimulation() + { + if (m_simulationFinishEvent.IsConnected()) + { + m_simulationFinishEvent.Disconnect(); + SetSimulationPaused(true); + m_numberOfPhysicsSteps = 0; + } + } + + void SimulationManager::SetSimulationPaused(bool paused) + { + // get az physics system + auto* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "Physics system is not available"); + const auto& sceneHandlers = physicsSystem->GetAllScenes(); + [[maybe_unused]] auto* sceneInterface = AZ::Interface::Get(); + AZ_Assert(sceneInterface, "Physics scene interface is not available"); + for (auto& scene : sceneHandlers) + { + AZ_Assert(scene, "Physics scene is not available"); + scene->SetEnabled(!paused); + m_isSimulationPaused = paused; + } + } + + void SimulationManager::StepSimulation(AZ::u64 steps) + { + if (steps == 0) + { + return; + } + m_numberOfPhysicsSteps = steps; + + // install handler + m_simulationFinishEvent = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler( + [this](AzPhysics::SceneHandle sceneHandle, float) + { + m_numberOfPhysicsSteps--; + AZ_Printf("SimulationManager", "Physics simulation step finished. Remaining steps: %d", m_numberOfPhysicsSteps); + SimulationManagerNotificationsBus::Broadcast( + &SimulationManagerNotifications::OnSimulationStepFinish, m_numberOfPhysicsSteps); + if (m_numberOfPhysicsSteps <= 0) + { + SetSimulationPaused(true); + // remove handler + m_simulationFinishEvent.Disconnect(); + } + }); + + // get default scene + [[maybe_unused]] auto* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "Physics system is not available"); + auto* sceneInterface = AZ::Interface::Get(); + AZ_Assert(sceneInterface, "Physics scene interface is not available"); + AzPhysics::SceneHandle defaultScene = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName); + + auto scene = sceneInterface->GetScene(defaultScene); + AZ_Assert(scene, "Default physics scene is not available"); + + // install handler + scene->RegisterSceneSimulationFinishHandler(m_simulationFinishEvent); + 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& result) + { + AZ_Trace("SimulationManager", "Delete all entities completed: %s, reload level", result.IsSuccess() ? "true" : "false"); + const char* levelName = AZ::Interface::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; + } + // reset of the simulation, assign the same state as at the beginning + InitializeSimulationState(); + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); + } + + SimulationState SimulationManager::GetSimulationState() const + { + return m_simulationState; + } + + AZ::Outcome SimulationManager::SetSimulationState(SimulationState stateToSet) + { + // check if simulation is in desire state + if (m_simulationState == stateToSet) + { + return AZ::Failure(FailedResult( + simulation_interfaces::srv::SetSimulationState::Response::ALREADY_IN_TARGET_STATE, + "Simulation is already in requested state, transition unecessary")); + } + + if (IsTransitionForbiddenInEditor(stateToSet)) + { + const auto stateToSetName = GetStateName(stateToSet); + const auto currentStateName = GetStateName(m_simulationState); + return AZ::Failure(FailedResult( + simulation_interfaces::srv::SetSimulationState::Response::INCORRECT_TRANSITION, + AZStd::string::format( + "Requested transition (%s -> %s) is forbidden in the Editor. It is available in GameLauncher.", + currentStateName.c_str(), + stateToSetName.c_str()))); + } + if (IsTransitionForbidden(stateToSet)) + { + const auto stateToSetName = GetStateName(stateToSet); + const auto currentStateName = GetStateName(m_simulationState); + return AZ::Failure(FailedResult( + simulation_interfaces::srv::SetSimulationState::Response::INCORRECT_TRANSITION, + AZStd::string::format("Requested transition (%s -> %s) is forbidden", currentStateName.c_str(), stateToSetName.c_str()))); + } + + switch (stateToSet) + { + case simulation_interfaces::msg::SimulationState::STATE_STOPPED: + { + SimulationManagerRequests::ReloadLevelCallback cb = []() + { + SimulationInterfaces::SimulationManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationManagerRequests::SetSimulationPaused, true); + }; + ReloadLevel(cb); + break; + } + case simulation_interfaces::msg::SimulationState::STATE_PLAYING: + { + SetSimulationPaused(false); + break; + } + case simulation_interfaces::msg::SimulationState::STATE_PAUSED: + { + SetSimulationPaused(true); + break; + } + case simulation_interfaces::msg::SimulationState::STATE_QUITTING: + { + // stop simulation -> kill the simulator. + SetSimulationPaused(true); + + // queue to allow status of this method to be returned, then start quitting + AZ::SystemTickBus::QueueFunction( + []() + { + AzFramework::ConsoleRequestBus::Broadcast(&AzFramework::ConsoleRequests::ExecuteConsoleCommand, "quit"); + }); + break; + } + default: + { + return AZ::Failure(FailedResult( + simulation_interfaces::srv::SetSimulationState::Response::INCORRECT_TRANSITION, "Requested state doesn't exists")); + break; + } + } + m_simulationState = stateToSet; + return AZ::Success(); + } + + bool SimulationManager::IsTransitionForbiddenInEditor(SimulationState requestedState) + { + // in the Editor we cannot reload level, so going to STOPPED state is forbidden, we cannot quit the editor so going to QUITTING + // state is forbidden + AZ::ApplicationTypeQuery appType; + AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationBus::Events::QueryApplicationType, appType); + if (appType.IsValid() && !appType.IsGame()) + { + if (requestedState == simulation_interfaces::msg::SimulationState::STATE_STOPPED || + requestedState == simulation_interfaces::msg::SimulationState::STATE_QUITTING) + { + return true; + } + } + return false; + } + + bool SimulationManager::IsTransitionForbidden(SimulationState requestedState) + { + AZStd::pair desireTransition{ m_simulationState, requestedState }; + auto it = AZStd::find(m_forbiddenStatesTransitions.begin(), m_forbiddenStatesTransitions.end(), desireTransition); + return it != m_forbiddenStatesTransitions.end(); + } + + void SimulationManager::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + DebugDraw::DebugDrawRequestBus::Broadcast( + &DebugDraw::DebugDrawRequests::DrawTextOnScreen, + AZStd::string::format("Simulation state: %s", GetStateName(m_simulationState).c_str()), + AZ::Color(1.0f, 1.0f, 1.0f, 1.0f), + 0.f); + } + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h new file mode 100644 index 0000000000..99cc15239e --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -0,0 +1,87 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + class SimulationManager + : public AZ::Component + , protected SimulationManagerRequestBus::Handler + , protected AzFramework::LevelSystemLifecycleNotificationBus::Handler + , protected AZ::TickBus::Handler + { + public: + AZ_COMPONENT_DECL(SimulationManager); + + static void Reflect(AZ::ReflectContext* context); + + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + SimulationManager(); + ~SimulationManager(); + + // AZ::Component + void Activate() override; + void Deactivate() override; + + private: + // 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; + SimulationState GetSimulationState() const override; + AZ::Outcome SetSimulationState(SimulationState stateToSet) override; + + // LevelSystemLifecycleNotificationBus interface implementation + void OnLoadingComplete(const char* levelName) override; + + // AZ::TickBus::Handler + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + bool m_isSimulationPaused = false; + + uint64_t m_numberOfPhysicsSteps = 0; + AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; + SimulationManagerRequests::ReloadLevelCallback m_reloadLevelCallback; + SimulationState m_simulationState{ + simulation_interfaces::msg::SimulationState::STATE_STOPPED + }; // default simulation state based on standard + void InitializeSimulationState(); + + bool IsTransitionForbiddenInEditor(SimulationState requestedState); + + bool IsTransitionForbidden(SimulationState requestedState); + // forbidden transition between state, first is current state, second is desire state + const AZStd::array, 4> m_forbiddenStatesTransitions{ { + { simulation_interfaces::msg::SimulationState::STATE_STOPPED, simulation_interfaces::msg::SimulationState::STATE_PAUSED }, + { simulation_interfaces::msg::SimulationState::STATE_QUITTING, simulation_interfaces::msg::SimulationState::STATE_STOPPED }, + { simulation_interfaces::msg::SimulationState::STATE_QUITTING, simulation_interfaces::msg::SimulationState::STATE_PLAYING }, + { simulation_interfaces::msg::SimulationState::STATE_QUITTING, simulation_interfaces::msg::SimulationState::STATE_PAUSED }, + } }; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h b/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h new file mode 100644 index 0000000000..4f6d60b811 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h @@ -0,0 +1,28 @@ +/* + * 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 +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + // common interface to store all simulation feature ros2 handlers in common container + class IROS2HandlerBase + { + public: + using SimulationFeatureType = ROS2SimulationInterfaces::SimulationFeatureType; + virtual ~IROS2HandlerBase() = default; + virtual AZStd::unordered_set GetProvidedFeatures() = 0; + virtual AZStd::string_view GetTypeName() const = 0; + virtual AZStd::string_view GetDefaultName() const = 0; + virtual void Initialize(rclcpp::Node::SharedPtr& node) = 0; + }; +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp new file mode 100644 index 0000000000..13ea23d75d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -0,0 +1,46 @@ +/* + * 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 "DeleteEntityServiceHandler.h" +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set DeleteEntityServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::DELETING }; + } + + AZStd::optional DeleteEntityServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + AZStd::string entityName = request.entity.c_str(); + + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::DeleteEntity, + entityName, + [this](const AZ::Outcome& outcome) + { + Response response; + if (outcome.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + } + else + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.c_str(); + } + SendResponse(response); + }); + return AZStd::nullopt; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h new file mode 100644 index 0000000000..2f587a27fc --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.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 +#include + +namespace ROS2SimulationInterfaces +{ + + class DeleteEntityServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "DeleteEntity"; + } + + AZStd::string_view GetDefaultName() const override + { + return "delete_entity"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp new file mode 100644 index 0000000000..d1654e0eae --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -0,0 +1,64 @@ +/* + * 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 "GetEntitiesServiceHandler.h" +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + AZStd::unordered_set GetEntitiesServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, + SimulationFeatures::ENTITY_BOUNDS_BOX, + SimulationFeatures::ENTITY_BOUNDS_CONVEX, + SimulationFeatures::ENTITY_CATEGORIES }; + } + + AZStd::optional GetEntitiesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + AZ::Outcome outcome; + + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + + const auto getFilterResult = Utils::GetEntityFiltersFromRequest(request); + if (!getFilterResult.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + response.result.error_message = getFilterResult.GetError().c_str(); + return response; + } + const SimulationInterfaces::EntityFilters filter = getFilterResult.GetValue(); + SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( + outcome, &SimulationInterfaces::SimulationEntityManagerRequests::GetEntities, filter); + std::vector& stdEntities = response.entities; + if (!outcome.IsSuccess()) + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.c_str(); + return response; + } + + const auto& entityNameList = outcome.GetValue(); + AZStd::transform( + entityNameList.begin(), + entityNameList.end(), + std::back_inserter(stdEntities), + [](const AZStd::string& entityName) + { + return entityName.c_str(); + }); + response.entities = stdEntities; + + return response; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h new file mode 100644 index 0000000000..bea01df183 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h @@ -0,0 +1,34 @@ +/* + * 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 +#include + +namespace ROS2SimulationInterfaces +{ + class GetEntitiesServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "GetEntities"; + } + + AZStd::string_view GetDefaultName() const override + { + return "get_entities"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp new file mode 100644 index 0000000000..c28d3ed771 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -0,0 +1,73 @@ +/* + * 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 "GetEntitiesStatesServiceHandler.h" +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + AZStd::unordered_set GetEntitiesStatesServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, + SimulationFeatures::ENTITY_BOUNDS_BOX, + SimulationFeatures::ENTITY_BOUNDS_CONVEX, + SimulationFeatures::ENTITY_CATEGORIES, + SimulationFeatures::ENTITY_STATE_GETTING }; + } + + AZStd::optional GetEntitiesStatesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + AZ::Outcome outcome; + + GetEntitiesStatesServiceHandler::Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + + const auto getFilterResult = Utils::GetEntityFiltersFromRequest(request); + if (!getFilterResult.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + response.result.error_message = getFilterResult.GetError().c_str(); + return response; + } + SimulationInterfaces::EntityFilters filter = getFilterResult.GetValue(); + SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( + outcome, &SimulationInterfaces::SimulationEntityManagerRequests::GetEntitiesStates, filter); + + if (!outcome.IsSuccess()) + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.c_str(); + return response; + } + + const auto& multipleEntitiesStates = outcome.GetValue(); + + 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; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h new file mode 100644 index 0000000000..02b52d7b9d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.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 +#include + +namespace ROS2SimulationInterfaces +{ + + class GetEntitiesStatesServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "GetEntitiesStates"; + } + + AZStd::string_view GetDefaultName() const override + { + return "get_entities_states"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp new file mode 100644 index 0000000000..423fb1be27 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -0,0 +1,54 @@ +/* + * 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 "GetEntityStateServiceHandler.h" +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set GetEntityStateServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_GETTING }; + } + + AZStd::optional GetEntityStateServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + AZStd::string entityName = request.entity.c_str(); + AZ::Outcome outcome; + + SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( + outcome, &SimulationInterfaces::SimulationEntityManagerRequests::GetEntityState, entityName); + + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + if (!outcome.IsSuccess()) + { + const auto& failedResult = outcome.GetError(); + response.result.result = 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 = ""; + entityStateMsg.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); + 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; + return response; + } + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h new file mode 100644 index 0000000000..04855469c2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h @@ -0,0 +1,34 @@ +/* + * 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 +#include + +namespace ROS2SimulationInterfaces +{ + + class GetEntityStateServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "GetEntityState"; + } + + AZStd::string_view GetDefaultName() const override + { + return "get_entity_state"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp new file mode 100644 index 0000000000..e4376c8fe8 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -0,0 +1,49 @@ +/* + * 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 "GetSimulationFeaturesServiceHandler.h" +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() + { + // standard doesn't define specific feature id for this service + 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; + ROS2SimulationInterfacesRequestBus::BroadcastResult(ros2Interfaces, &ROS2SimulationInterfacesRequests::GetSimulationFeatures); + // call bus to get simulation features on SimulationInterfaces Gem side + 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 + Response response; + for (auto id : o3deInterfaces) + { + if (ros2Interfaces.contains(SimulationFeatureType(id))) + { + response.features.features.emplace_back(id); + } + } + // sort features for better readability + AZStd::sort(response.features.features.begin(), response.features.features.end()); + return response; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h new file mode 100644 index 0000000000..6c8c4fbab2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -0,0 +1,36 @@ +/* + * 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 +#include +#include + +namespace ROS2SimulationInterfaces +{ + + class GetSimulationFeaturesServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "GetSimulationFeatures"; + } + + AZStd::string_view GetDefaultName() const override + { + return "get_simulation_features"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp new file mode 100644 index 0000000000..b4f3510103 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp @@ -0,0 +1,32 @@ +/* + * 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 "GetSimulationStateServiceHandler.h" +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + AZStd::unordered_set GetSimulationStateServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_GETTING }; + } + + AZStd::optional GetSimulationStateServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + Response response; + SimulationInterfaces::SimulationState currentState; + SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult( + currentState, &SimulationInterfaces::SimulationManagerRequests::GetSimulationState); + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + response.state.state = currentState; + return response; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h new file mode 100644 index 0000000000..ff923a129d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.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 "Services/ROS2ServiceBase.h" +#include +#include + +namespace ROS2SimulationInterfaces +{ + + class GetSimulationStateServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "GetSimulationState"; + } + + AZStd::string_view GetDefaultName() const override + { + return "get_simulation_state"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp new file mode 100644 index 0000000000..d2f2e87de4 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -0,0 +1,51 @@ +/* + * 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 "GetSpawnablesServiceHandler.h" +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set GetSpawnablesServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SPAWNABLES }; + } + + AZStd::optional GetSpawnablesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + AZ::Outcome outcome; + SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( + outcome, &SimulationInterfaces::SimulationEntityManagerRequests::GetSpawnables); + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + if (!outcome.IsSuccess()) + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.c_str(); + return response; + } + + const auto& spawnableList = outcome.GetValue(); + AZStd::transform( + spawnableList.begin(), + spawnableList.end(), + AZStd::back_inserter(response.spawnables), + [](const SimulationInterfaces::Spawnable& spawnable) + { + simulation_interfaces::msg::Spawnable simSpawnable; + simSpawnable.uri = spawnable.m_uri.c_str(); + simSpawnable.description = spawnable.m_description.c_str(); + return simSpawnable; + }); + + return response; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h new file mode 100644 index 0000000000..69159cc415 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.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 +#include + +namespace ROS2SimulationInterfaces +{ + + class GetSpawnablesServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "GetSpawnables"; + } + + AZStd::string_view GetDefaultName() const override + { + return "get_spawnables"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h new file mode 100644 index 0000000000..10259da1ea --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h @@ -0,0 +1,91 @@ +/* + * 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 +#include +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + //! Base for each ROS 2 service handler, forces declaration of features provided by the handler + //! combined information along all ROS 2 handlers gives information about simulation features + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures; + template + class ROS2ServiceBase : public virtual IROS2HandlerBase + { + public: + using Request = typename RosServiceType::Request; + using Response = typename RosServiceType::Response; + using ServiceHandle = std::shared_ptr>; + virtual ~ROS2ServiceBase() = default; + + void Initialize(rclcpp::Node::SharedPtr& node) override + { + CreateService(node); + } + + void SendResponse(Response response) + { + AZ_Assert(m_serviceHandle, "Failed to get m_serviceHandle"); + AZ_Assert(m_lastRequestHeader, "Failed to get last request header ptr"); + m_serviceHandle->send_response(*m_lastRequestHeader, response); + } + + protected: + //! This function is called when a service request is received. + virtual AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) = 0; + + //! return features id defined by the handler, ids must follow the definition inside standard: + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + AZStd::unordered_set GetProvidedFeatures() override + { + return {}; + }; + + private: + void CreateService(rclcpp::Node::SharedPtr& node) + { + // get the service name from the type name + AZStd::string serviceName = RegistryUtilities::GetName(GetTypeName()); + + if (serviceName.empty()) + { + // if the service name is empty, use the default name + serviceName = GetDefaultName(); + } + + const std::string serviceNameStr{ serviceName.c_str(), serviceName.size() }; + m_serviceHandle = node->create_service( + serviceNameStr, + [this]( + const ServiceHandle service_handle, + const std::shared_ptr header, + const std::shared_ptr request) + { + m_lastRequestHeader = header; + auto response = HandleServiceRequest(header, *request); + // if no response passed it means, that handleServiceRequest will send response in defined callback after time consuming + // task, header needs to be cached + if (response.has_value()) + { + service_handle->send_response(*header, response.value()); + } + }); + } + + std::shared_ptr m_lastRequestHeader = nullptr; + ServiceHandle m_serviceHandle; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp new file mode 100644 index 0000000000..7f488c2c8c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -0,0 +1,123 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set ResetSimulationServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SIMULATION_RESET, + SimulationFeatures::SIMULATION_RESET_TIME, + SimulationFeatures::SIMULATION_RESET_STATE, + SimulationFeatures::SIMULATION_RESET_SPAWNED }; + } + + AZStd::optional ResetSimulationServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + if (request.scope == Request::SCOPE_STATE) + { + Response response; + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::ResetAllEntitiesToInitialState); + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + return response; + } + + if (request.scope == Request::SCOPE_SPAWNED) + { + auto deletionCompletedCb = [this](const AZ::Outcome& outcome) + { + Response response; + if (outcome.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + } + else + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.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; + } + + // 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 diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h new file mode 100644 index 0000000000..c3de46c9f8 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h @@ -0,0 +1,34 @@ +/* + * 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 +#include + +namespace ROS2SimulationInterfaces +{ + class ResetSimulationServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "ResetSimulation"; + } + + AZStd::string_view GetDefaultName() const override + { + return "reset_simulation"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp new file mode 100644 index 0000000000..f701ad60d1 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -0,0 +1,45 @@ +/* + * 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 "SetEntityStateServiceHandler.h" +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set SetEntityStateServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_SETTING }; + } + + AZStd::optional SetEntityStateServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + AZ::Outcome outcome; + AZStd::string entityName = request.entity.c_str(); + SimulationInterfaces::EntityState entityState; + entityState.m_pose = ROS2::ROS2Conversions::FromROS2Pose(request.state.pose); + 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); + + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + if (!outcome.IsSuccess()) + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.c_str(); + } + + return response; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h new file mode 100644 index 0000000000..f096580556 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.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 +#include + +namespace ROS2SimulationInterfaces +{ + + class SetEntityStateServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "SetEntityState"; + } + + AZStd::string_view GetDefaultName() const override + { + return "set_entity_state"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp new file mode 100644 index 0000000000..b713a420e5 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -0,0 +1,42 @@ +/* + * 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 "SetSimulationStateServiceHandler.h" +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + AZStd::unordered_set SetSimulationStateServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_SETTING, + SimulationFeatures::SIMULATION_STATE_PAUSE }; + } + + AZStd::optional SetSimulationStateServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + Response response; + AZ::Outcome transitionResult; + SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult( + transitionResult, + &SimulationInterfaces::SimulationManagerRequests::SetSimulationState, + SimulationInterfaces::SimulationState(request.state.state)); + if (transitionResult.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + } + else + { + response.result.result = transitionResult.GetError().m_errorCode; + response.result.error_message = transitionResult.GetError().m_errorString.c_str(); + } + return response; + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h new file mode 100644 index 0000000000..23c55d8998 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.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 "Services/ROS2ServiceBase.h" +#include +#include + +namespace ROS2SimulationInterfaces +{ + + class SetSimulationStateServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "SetSimulationState"; + } + + AZStd::string_view GetDefaultName() const override + { + return "set_simulation_state"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp new file mode 100644 index 0000000000..a9cef9f4f4 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -0,0 +1,92 @@ +/* + * 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 "SpawnEntityServiceHandler.h" +#include +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set SpawnEntityServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SPAWNING }; + } + + AZStd::optional SpawnEntityServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + const AZStd::string_view name{ request.name.c_str(), request.name.size() }; + const AZStd::string_view uri{ request.uri.c_str(), request.uri.size() }; + const AZStd::string_view entityNamespace{ request.entity_namespace.c_str(), request.entity_namespace.size() }; + + // Validate entity name + if (!name.empty() && !ValidateEntityName(name)) + { + Response response; + response.result.result = simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID; + response.result.error_message = "Invalid entity name. Entity names can only contain alphanumeric characters and underscores."; + SendResponse(response); + return AZStd::nullopt; + } + + // Validate namespace name + if (!entityNamespace.empty() && !ValidateNamespaceName(entityNamespace)) + { + 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."; + SendResponse(response); + return AZStd::nullopt; + } + + const AZ::Transform initialPose = ROS2::ROS2Conversions::FromROS2Pose(request.initial_pose.pose); + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::SpawnEntity, + name, + uri, + entityNamespace, + initialPose, + request.allow_renaming, + [this](const AZ::Outcome& outcome) + { + Response response; + if (outcome.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + response.entity_name = outcome.GetValue().c_str(); + } + else + { + const auto& failedResult = outcome.GetError(); + response.result.result = failedResult.m_errorCode; + response.result.error_message = failedResult.m_errorString.c_str(); + } + SendResponse(response); + }); + return AZStd::nullopt; + } + + bool SpawnEntityServiceHandler::ValidateEntityName(const AZStd::string& entityName) + { + const AZStd::regex entityRegex{ R"(^[a-zA-Z0-9_]+$)" }; // Entity names can only contain alphanumeric characters and underscores + return AZStd::regex_match(entityName, entityRegex); + } + + bool SpawnEntityServiceHandler::ValidateNamespaceName(const AZStd::string& namespaceName) + { + const AZStd::regex namespaceRegex{ + R"(^[a-zA-Z0-9_/]+$)" + }; // Namespace names can only contain alphanumeric characters and underscores and forward slashes + return AZStd::regex_match(namespaceName, namespaceRegex); + } + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h new file mode 100644 index 0000000000..26cba7d3ec --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h @@ -0,0 +1,38 @@ +/* + * 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 +#include +#include +namespace ROS2SimulationInterfaces +{ + class SpawnEntityServiceHandler : public ROS2ServiceBase + { + public: + AZStd::string_view GetTypeName() const override + { + return "SpawnEntity"; + } + + AZStd::string_view GetDefaultName() const override + { + return "spawn_entity"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + + private: + bool ValidateEntityName(const AZStd::string& entityName); + bool ValidateNamespaceName(const AZStd::string& namespaceName); + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp new file mode 100644 index 0000000000..fdc894e7eb --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp @@ -0,0 +1,56 @@ +/* + * 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 "StepSimulationServiceHandler.h" +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + + AZStd::unordered_set StepSimulationServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE }; + } + + AZStd::optional StepSimulationServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) + { + bool isPaused = false; + SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult( + isPaused, &SimulationInterfaces::SimulationManagerRequests::IsSimulationPaused); + if (!isPaused) + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + response.result.error_message = "Request cannot be processed - simulation has to be paused."; + return response; + } + + SimulationInterfaces::SimulationManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationManagerRequests::StepSimulation, request.steps); + AZ::TickBus::Handler::BusConnect(); + return AZStd::nullopt; + } + + void StepSimulationServiceHandler::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + bool isActive = true; + SimulationInterfaces::SimulationManagerRequestBus::BroadcastResult( + isActive, &SimulationInterfaces::SimulationManagerRequests::IsSimulationStepsActive); + if (!isActive) + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + SendResponse(response); + AZ::TickBus::Handler::BusDisconnect(); + } + } +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h new file mode 100644 index 0000000000..a94494c146 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h @@ -0,0 +1,41 @@ +/* + * 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 +#include +#include +namespace ROS2SimulationInterfaces +{ + + class StepSimulationServiceHandler + : public ROS2ServiceBase + , private AZ::TickBus::Handler + { + public: + AZStd::string_view GetTypeName() const override + { + return "StepSimulation"; + } + + AZStd::string_view GetDefaultName() const override + { + return "step_simulation"; + } + AZStd::unordered_set GetProvidedFeatures() override; + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; + + private: + // AZ::TickBus::Handler overrides + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + }; + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp new file mode 100644 index 0000000000..61712def0d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -0,0 +1,47 @@ +/* + * 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 "SimulationInterfacesModuleInterface.h" +#include + +#include + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + AZ_TYPE_INFO_WITH_NAME_IMPL( + SimulationInterfacesModuleInterface, "SimulationInterfacesModuleInterface", SimulationInterfacesModuleInterfaceTypeId); + AZ_RTTI_NO_TYPE_INFO_IMPL(SimulationInterfacesModuleInterface, AZ::Module); + AZ_CLASS_ALLOCATOR_IMPL(SimulationInterfacesModuleInterface, AZ::SystemAllocator); + + SimulationInterfacesModuleInterface::SimulationInterfacesModuleInterface() + { + m_descriptors.insert( + m_descriptors.end(), + { + SimulationEntitiesManager::CreateDescriptor(), + SimulationManager::CreateDescriptor(), + SimulationFeaturesAggregator::CreateDescriptor(), + ROS2SimulationInterfaces::ROS2SimulationInterfacesSystemComponent::CreateDescriptor(), + }); + } + + AZ::ComponentTypeList SimulationInterfacesModuleInterface::GetRequiredSystemComponents() const + { + return AZ::ComponentTypeList{ + azrtti_typeid(), + azrtti_typeid(), + azrtti_typeid(), + azrtti_typeid(), + }; + } +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.h b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.h new file mode 100644 index 0000000000..6e9825a67e --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.h @@ -0,0 +1,30 @@ +/* + * 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 +#include +#include +#include + +namespace SimulationInterfaces +{ + class SimulationInterfacesModuleInterface : public AZ::Module + { + public: + AZ_TYPE_INFO_WITH_NAME_DECL(SimulationInterfacesModuleInterface) + AZ_RTTI_NO_TYPE_INFO_DECL() + AZ_CLASS_ALLOCATOR_DECL + + SimulationInterfacesModuleInterface(); + + /** + * Add required SystemComponents to the SystemEntity. + */ + AZ::ComponentTypeList GetRequiredSystemComponents() const override; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp new file mode 100644 index 0000000000..b56503c1b2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp @@ -0,0 +1,87 @@ +/* + * 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 "ROS2SimulationInterfacesEditorSystemComponent.h" +#include + +#include + +namespace ROS2SimulationInterfaces +{ + AZ_COMPONENT_IMPL( + ROS2SimulationInterfacesEditorSystemComponent, + "ROS2SimulationInterfacesEditorSystemComponent", + ROS2SimulationInterfacesEditorSystemComponentTypeId, + BaseSystemComponent); + + void ROS2SimulationInterfacesEditorSystemComponent::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + ROS2SimulationInterfacesEditorSystemComponent::ROS2SimulationInterfacesEditorSystemComponent() + : m_nodeHandler( + [this](std::shared_ptr node) + { + if (!m_systemComponentActivated) + { + ROS2SimulationInterfacesSystemComponent::Activate(); + m_systemComponentActivated = true; + } + else + { + ROS2SimulationInterfacesSystemComponent::Deactivate(); + m_systemComponentActivated = false; + } + }) + { + } + + ROS2SimulationInterfacesEditorSystemComponent::~ROS2SimulationInterfacesEditorSystemComponent() = default; + + void ROS2SimulationInterfacesEditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + BaseSystemComponent::GetProvidedServices(provided); + provided.push_back(AZ_CRC_CE("ROS2SimulationInterfacesEditorService")); + } + + void ROS2SimulationInterfacesEditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + BaseSystemComponent::GetIncompatibleServices(incompatible); + incompatible.push_back(AZ_CRC_CE("ROS2SimulationInterfacesEditorService")); + } + + void ROS2SimulationInterfacesEditorSystemComponent::GetRequiredServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + BaseSystemComponent::GetRequiredServices(required); + required.push_back(AZ_CRC_CE("ROS2EditorService")); + } + + void ROS2SimulationInterfacesEditorSystemComponent::GetDependentServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + } + + void ROS2SimulationInterfacesEditorSystemComponent::Activate() + { + AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + ROS2::ROS2Interface::Get()->ConnectOnNodeChanged(m_nodeHandler); + } + + void ROS2SimulationInterfacesEditorSystemComponent::Deactivate() + { + m_nodeHandler.Disconnect(); + AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); + } + +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h new file mode 100644 index 0000000000..8a395e31d5 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h @@ -0,0 +1,47 @@ +/* + * 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 + +#include +#include +#include + +namespace ROS2SimulationInterfaces +{ + /// System component for ROS2SimulationInterfaces editor + class ROS2SimulationInterfacesEditorSystemComponent + : public ROS2SimulationInterfacesSystemComponent + , protected AzToolsFramework::EditorEvents::Bus::Handler + { + using BaseSystemComponent = ROS2SimulationInterfacesSystemComponent; + + public: + AZ_COMPONENT_DECL(ROS2SimulationInterfacesEditorSystemComponent); + + static void Reflect(AZ::ReflectContext* context); + + ROS2SimulationInterfacesEditorSystemComponent(); + ~ROS2SimulationInterfacesEditorSystemComponent(); + + private: + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + // AZ::Component + void Activate() override; + void Deactivate() override; + + bool m_systemComponentActivated = false; + ROS2::ROS2Requests::NodeChangedEvent::Handler m_nodeHandler; + }; +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp new file mode 100644 index 0000000000..5c276d7a11 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp @@ -0,0 +1,67 @@ +/* + * 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 "SimulationEntitiesManagerEditor.h" +#include + +#include + +namespace SimulationInterfaces +{ + AZ_COMPONENT_IMPL( + SimulationEntitiesManagerEditor, "SimulationEntitiesManagerEditor", SimulationEntitiesManagerEditorTypeId, BaseSystemComponent); + + void SimulationEntitiesManagerEditor::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + SimulationEntitiesManagerEditor::SimulationEntitiesManagerEditor() = default; + + SimulationEntitiesManagerEditor::~SimulationEntitiesManagerEditor() = default; + + void SimulationEntitiesManagerEditor::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + BaseSystemComponent::GetProvidedServices(provided); + provided.push_back(AZ_CRC_CE("SimulationInterfacesEditorService")); + } + + void SimulationEntitiesManagerEditor::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + BaseSystemComponent::GetIncompatibleServices(incompatible); + incompatible.push_back(AZ_CRC_CE("SimulationInterfacesEditorService")); + } + + void SimulationEntitiesManagerEditor::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + BaseSystemComponent::GetRequiredServices(required); + required.push_back(AZ_CRC_CE("SimulationFeaturesAggregatorEditorService")); + } + + void SimulationEntitiesManagerEditor::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + dependent.push_back(AZ_CRC_CE("SimulationFeaturesAggregatorEditorService")); + } + + void SimulationEntitiesManagerEditor::Activate() + { + SimulationEntitiesManager::Activate(); + AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + } + + void SimulationEntitiesManagerEditor::Deactivate() + { + AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); + SimulationEntitiesManager::Deactivate(); + } + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.h b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.h new file mode 100644 index 0000000000..b04108822e --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.h @@ -0,0 +1,42 @@ +/* + * 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 + +#include + +namespace SimulationInterfaces +{ + /// System component for SimulationInterfaces editor + class SimulationEntitiesManagerEditor + : public SimulationEntitiesManager + , protected AzToolsFramework::EditorEvents::Bus::Handler + { + using BaseSystemComponent = SimulationEntitiesManager; + + public: + AZ_COMPONENT_DECL(SimulationEntitiesManagerEditor); + + static void Reflect(AZ::ReflectContext* context); + + SimulationEntitiesManagerEditor(); + ~SimulationEntitiesManagerEditor(); + + private: + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + // AZ::Component + void Activate() override; + void Deactivate() override; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.cpp new file mode 100644 index 0000000000..810011a72d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.cpp @@ -0,0 +1,67 @@ +/* + * 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 "SimulationFeaturesAggregatorEditor.h" +#include + +#include + +namespace SimulationInterfaces +{ + AZ_COMPONENT_IMPL( + SimulationFeaturesAggregatorEditor, "SimulationMangerEditor", SimulationFeaturesAggregatorEditorTypeId, BaseSystemComponent); + + void SimulationFeaturesAggregatorEditor::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + SimulationFeaturesAggregatorEditor::SimulationFeaturesAggregatorEditor() = default; + + SimulationFeaturesAggregatorEditor::~SimulationFeaturesAggregatorEditor() = default; + + void SimulationFeaturesAggregatorEditor::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + BaseSystemComponent::GetProvidedServices(provided); + provided.push_back(AZ_CRC_CE("SimulationFeaturesAggregatorEditorService")); + } + + void SimulationFeaturesAggregatorEditor::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + BaseSystemComponent::GetIncompatibleServices(incompatible); + incompatible.push_back(AZ_CRC_CE("SimulationFeaturesAggregatorEditorService")); + } + + void SimulationFeaturesAggregatorEditor::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + BaseSystemComponent::GetRequiredServices(required); + } + + void SimulationFeaturesAggregatorEditor::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + } + void SimulationFeaturesAggregatorEditor::Init() + { + BaseSystemComponent::Init(); + } + + void SimulationFeaturesAggregatorEditor::Activate() + { + BaseSystemComponent::Activate(); + } + + void SimulationFeaturesAggregatorEditor::Deactivate() + { + BaseSystemComponent::Deactivate(); + } + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.h b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.h new file mode 100644 index 0000000000..f52f2c1ee2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.h @@ -0,0 +1,41 @@ +/* + * 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 + +#include + +namespace SimulationInterfaces +{ + /// System component for SimulationInterfaces editor + class SimulationFeaturesAggregatorEditor : public SimulationFeaturesAggregator + { + using BaseSystemComponent = SimulationFeaturesAggregator; + + public: + AZ_COMPONENT_DECL(SimulationFeaturesAggregatorEditor); + + static void Reflect(AZ::ReflectContext* context); + + SimulationFeaturesAggregatorEditor(); + ~SimulationFeaturesAggregatorEditor(); + + private: + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + // AZ::Component + void Init() override; + void Activate() override; + void Deactivate() override; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp new file mode 100644 index 0000000000..1f308adbde --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp @@ -0,0 +1,47 @@ +/* + * 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 "ROS2SimulationInterfacesEditorSystemComponent.h" +#include "SimulationEntitiesManagerEditor.h" +#include "SimulationFeaturesAggregatorEditor.h" +#include "SimulationManagerEditor.h" +#include +#include +namespace SimulationInterfaces +{ + class SimulationInterfacesEditorModule : public SimulationInterfacesModuleInterface + { + public: + AZ_RTTI(SimulationInterfacesEditorModule, SimulationInterfacesEditorModuleTypeId, SimulationInterfacesModuleInterface); + AZ_CLASS_ALLOCATOR(SimulationInterfacesEditorModule, AZ::SystemAllocator); + + SimulationInterfacesEditorModule() + { + m_descriptors.insert( + m_descriptors.end(), + { + SimulationEntitiesManagerEditor::CreateDescriptor(), + SimulationManagerEditor::CreateDescriptor(), + SimulationFeaturesAggregatorEditor::CreateDescriptor(), + ROS2SimulationInterfaces::ROS2SimulationInterfacesEditorSystemComponent::CreateDescriptor(), + }); + } + + AZ::ComponentTypeList GetRequiredSystemComponents() const override + { + return AZ::ComponentTypeList{ + azrtti_typeid(), + azrtti_typeid(), + azrtti_typeid(), + azrtti_typeid(), + }; + } + }; +} // namespace SimulationInterfaces + +AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfaces_Editor, SimulationInterfaces::SimulationInterfacesEditorModule) diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp new file mode 100644 index 0000000000..98c40fd2f7 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp @@ -0,0 +1,76 @@ +/* + * 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 "SimulationManagerEditor.h" +#include + +#include + +namespace SimulationInterfaces +{ + AZ_COMPONENT_IMPL(SimulationManagerEditor, "SimulationMangerEditor", SimulationManagerEditorTypeId, BaseSystemComponent); + + void SimulationManagerEditor::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + SimulationManagerEditor::SimulationManagerEditor() = default; + + SimulationManagerEditor::~SimulationManagerEditor() = default; + + void SimulationManagerEditor::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + BaseSystemComponent::GetProvidedServices(provided); + provided.push_back(AZ_CRC_CE("SimulationManagerEditorService")); + } + + void SimulationManagerEditor::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + BaseSystemComponent::GetIncompatibleServices(incompatible); + incompatible.push_back(AZ_CRC_CE("SimulationManagerEditorService")); + } + + void SimulationManagerEditor::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + BaseSystemComponent::GetRequiredServices(required); + required.push_back(AZ_CRC_CE("SimulationFeaturesAggregatorEditorService")); + } + + void SimulationManagerEditor::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + dependent.push_back(AZ_CRC_CE("SimulationFeaturesAggregatorEditorService")); + } + void SimulationManagerEditor::Init() + { + BaseSystemComponent::Init(); + } + + void SimulationManagerEditor::Activate() + { + AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusConnect(); + } + + void SimulationManagerEditor::Deactivate() + { + AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusDisconnect(); + AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); + BaseSystemComponent::Deactivate(); + } + + void SimulationManagerEditor::OnStartPlayInEditorBegin() + { + BaseSystemComponent::Activate(); + } + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h new file mode 100644 index 0000000000..538a59c97a --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h @@ -0,0 +1,48 @@ +/* + * 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 +#include + +#include + +namespace SimulationInterfaces +{ + /// System component for SimulationInterfaces editor + class SimulationManagerEditor + : public SimulationManager + , protected AzToolsFramework::EditorEvents::Bus::Handler + , private AzToolsFramework::EditorEntityContextNotificationBus::Handler + { + using BaseSystemComponent = SimulationManager; + + public: + AZ_COMPONENT_DECL(SimulationManagerEditor); + + static void Reflect(AZ::ReflectContext* context); + + SimulationManagerEditor(); + ~SimulationManagerEditor(); + + private: + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); + + // AZ::Component + void Init() override; + void Activate() override; + void Deactivate() override; + + // EditorEntityContextNotificationBus + void OnStartPlayInEditorBegin() override; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp new file mode 100644 index 0000000000..7164a6db32 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp @@ -0,0 +1,24 @@ +/* + * 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 "RegistryUtils.h" +#include +#include + +namespace ROS2SimulationInterfaces::RegistryUtilities +{ + AZStd::string GetName(const AZStd::string& serviceType) + { + AZ::SettingsRegistryInterface* settingsRegistry = AZ::SettingsRegistry::Get(); + AZ_Assert(settingsRegistry, "Settings Registry is not available"); + AZStd::string output = ""; + AZ::IO::Path setRegPath = AZ::IO::Path(RegistryPrefix) / AZ::IO::Path(serviceType); + settingsRegistry->Get(output, setRegPath.String()); + return output; + } +} // namespace ROS2SimulationInterfaces::RegistryUtilities diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h new file mode 100644 index 0000000000..85e2c58288 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h @@ -0,0 +1,20 @@ +/* + * 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 + +namespace ROS2SimulationInterfaces::RegistryUtilities +{ + // prefix for settings registry related to ros2 topics names + inline constexpr const char* RegistryPrefix = "/ROS2SimulationInterfaces"; + + //! Gets name of the service with defined type form settings registry + //! @return string with service name, if setting registry doesn't exits returns empty string + [[nodiscard]] AZStd::string GetName(const AZStd::string& serviceType); +} // namespace ROS2SimulationInterfaces::RegistryUtilities diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h new file mode 100644 index 0000000000..0a544f0148 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h @@ -0,0 +1,64 @@ +/* + * 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 +#include +#include +#include + +namespace ROS2SimulationInterfaces::Utils +{ + template + AZ::Outcome GetEntityFiltersFromRequest(const RequestType& request) + { + SimulationInterfaces::EntityFilters filter; + filter.m_nameFilter = request.filters.filter.c_str(); + uint8_t type = request.filters.bounds.type; + if (type == simulation_interfaces::msg::Bounds::TYPE_BOX) + { + if (request.filters.bounds.points.size() != 2) + { + 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(); + 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."); + } + const auto min = ROS2::ROS2Conversions::FromROS2Vector3(p1); + const auto max = ROS2::ROS2Conversions::FromROS2Vector3(p2); + const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(min, max); + filter.m_boundsShape = AZStd::make_shared(aabb.GetExtents()); + } + else if (type == simulation_interfaces::msg::Bounds::TYPE_CONVEX_HULL) + { + if (request.filters.bounds.points.size() < 3) + { + return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have at least 3 points."); + } + // TODO: Implement convex hull shape configuration + // filter.m_boundsShape = AZStd::make_shared(); + return AZ::Failure("Unsupported type! Type 'TYPE_CONVEX_HULL' is not supported."); + } + else if (type == simulation_interfaces::msg::Bounds::TYPE_SPHERE) + { + if (request.filters.bounds.points.size() != 2) + { + return AZ::Failure("Invalid number of points! Type 'TYPE_SPHERE' should have exactly 2 points."); + } + 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)); + } +} // namespace ROS2SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp new file mode 100644 index 0000000000..c34504a122 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -0,0 +1,636 @@ + +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Mocks/SimulationEntityManagerMock.h" +#include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h" +#include "Mocks/SimulationManagerMock.h" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace UnitTest +{ + class SimulationInterfaceROS2TestEnvironment : public AZ::Test::GemTestEnvironment + { + // AZ::Test::GemTestEnvironment overrides ... + void AddGemsAndComponents() override; + AZ::ComponentApplication* CreateApplicationInstance() override; + void PostSystemEntityActivate() override; + + public: + SimulationInterfaceROS2TestEnvironment() = default; + ~SimulationInterfaceROS2TestEnvironment() override = default; + }; + + void SimulationInterfaceROS2TestEnvironment::AddGemsAndComponents() + { + using namespace ROS2SimulationInterfaces; + AddActiveGems(AZStd::to_array({ "ROS2" })); + AddDynamicModulePaths({ "ROS2" }); + AddComponentDescriptors(AZStd::initializer_list{ + ROS2SimulationInterfacesSystemComponent::CreateDescriptor(), + }); + AddRequiredComponents(AZStd::to_array({ ROS2SimulationInterfacesSystemComponent::TYPEINFO_Uuid() })); + } + + AZ::ComponentApplication* SimulationInterfaceROS2TestEnvironment::CreateApplicationInstance() + { + // Using ToolsTestApplication to have AzFramework and AzToolsFramework components. + return aznew UnitTest::ToolsTestApplication("SimulationInterfaceROS2TestEnvironment"); + } + + void SimulationInterfaceROS2TestEnvironment::PostSystemEntityActivate() + { + AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize); + } + + class SimulationInterfaceROS2TestFixture : public ::testing::Test + { + protected: + std::shared_ptr GetRos2Node() + { + auto interface = ROS2::ROS2Interface::Get(); + AZ_Assert(interface, "ROS2 interface is not available."); + auto node = interface->GetNode(); + AZ_Assert(node, "Node is not available."); + return node; + } + void SpinAppSome(int numberOfTicks = 50) + { + AZ::ComponentApplication* app = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(app, &AZ::ComponentApplicationBus::Events::GetApplication); + AZ_Assert(app, "Application pointer is not available."); + for (int i = 0; i < numberOfTicks; ++i) + { + app->Tick(); + } + } + + template + void SpinAppUntilFuture(Future& future) + { + AZ::ComponentApplication* app = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(app, &AZ::ComponentApplicationBus::Events::GetApplication); + AZ_Assert(app, "Application pointer is not available."); + constexpr int MaximumTicks = 1000; + for (int i = 0; i < MaximumTicks; ++i) + { + app->Tick(); + if (future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) + { + return; + } + } + } + }; + + //! Perform a smoke test to check if the ROS2 node is available from ROS2 gem + TEST_F(SimulationInterfaceROS2TestFixture, TestIfROS2NodeIsAvailable) + { + ASSERT_NE(GetRos2Node(), nullptr) << "ROS2 node is not available."; + } + + //! Perform a smoke test to check if the ROS2 domain is working + TEST_F(SimulationInterfaceROS2TestFixture, SmokeTestROS2Domain) + { + auto node = GetRos2Node(); + auto pub = node->create_publisher("hello", 20); + int receivedMsgs = 0; + auto sub = node->create_subscription( + "hello", + 10, + [&receivedMsgs](const std_msgs::msg::String& msg) + { + receivedMsgs++; + }); + std_msgs::msg::String msg; + + msg.data = "Hello World!"; + for (int i = 0; i < 10; ++i) + { + pub->publish(msg); + } + SpinAppSome(); + EXPECT_EQ(receivedMsgs, 10) << "Did not receive all messages."; + } + + //! Check if expected services are available and has the default name + TEST_F(SimulationInterfaceROS2TestFixture, TestIfServicesAvailableInROS2Domain) + { + auto node = GetRos2Node(); + auto services = node->get_service_names_and_types(); + ASSERT_FALSE(services.empty()) << "No services available."; + + EXPECT_NE(services.find("/delete_entity"), services.end()); + EXPECT_NE(services.find("/get_entities"), services.end()); + EXPECT_NE(services.find("/get_entities_states"), services.end()); + EXPECT_NE(services.find("/get_entity_state"), services.end()); + EXPECT_NE(services.find("/get_spawnables"), services.end()); + EXPECT_NE(services.find("/set_entity_state"), services.end()); + 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 + TEST_F(SimulationInterfaceROS2TestFixture, TestDeleteEntity_01) + { + using ::testing::_; + auto node = GetRos2Node(); + auto mock = std::make_shared(); + auto client = node->create_client("/delete_entity"); + auto request = std::make_shared(); + const char TestEntityName[] = "test_entity"; + request->entity = std::string(TestEntityName); + const AZStd::string entityName = AZStd::string(TestEntityName); + + EXPECT_CALL(*mock, DeleteEntity(entityName, _)) + .WillOnce(::testing::Invoke( + [](const AZStd::string& name, SimulationInterfaces::DeletionCompletedCb completedCb) + { + EXPECT_EQ(name, "test_entity"); + completedCb(AZ::Success()); + })); + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + 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::msg::Result::RESULT_OK); + } + + //! Test if the service call fails when the entity is not found + TEST_F(SimulationInterfaceROS2TestFixture, TestDeleteEntity_02) + { + using ::testing::_; + auto node = GetRos2Node(); + SimulationEntityManagerMockedHandler mock; + auto client = node->create_client("/delete_entity"); + auto request = std::make_shared(); + const char TestEntityName[] = "test_entity"; + request->entity = std::string(TestEntityName); + const AZStd::string entityName = AZStd::string(TestEntityName); + + EXPECT_CALL(mock, DeleteEntity(entityName, _)) + .WillOnce(::testing::Invoke( + [](const AZStd::string& name, SimulationInterfaces::DeletionCompletedCb completedCb) + { + EXPECT_EQ(name, "test_entity"); + FailedResult failedResult; + failedResult.m_errorCode = simulation_interfaces::msg::Result::RESULT_NOT_FOUND, + failedResult.m_errorString = "FooBar not found."; + completedCb(AZ::Failure(failedResult)); + })); + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + 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::msg::Result::RESULT_NOT_FOUND); + } + + //! Happy path test for GetEntities with a sphere shape + //! Test if the service is called with a sphere shape and Posix filter is passed through + TEST_F(SimulationInterfaceROS2TestFixture, GetEntitiesWithShapeSphereAndFilterValid) + { + using ::testing::_; + auto node = GetRos2Node(); + SimulationEntityManagerMockedHandler mock; + auto client = node->create_client("/get_entities"); + auto request = std::make_shared(); + request->filters.bounds.points.resize(2); + const AZ::Vector3 point1(1.0f, 2.0f, 3.0f); + request->filters.bounds.points[0].x = point1.GetX(); + request->filters.bounds.points[0].y = point1.GetY(); + request->filters.bounds.points[0].z = point1.GetZ(); + request->filters.bounds.points[1].x = 99.0f; + request->filters.bounds.type = simulation_interfaces::msg::Bounds::TYPE_SPHERE; + request->filters.filter = "FooBarFilter"; + + EXPECT_CALL(mock, GetEntities(_)) + .WillOnce(::testing::Invoke( + [=](const EntityFilters& filter) + { + EXPECT_NE(filter.m_boundsShape, nullptr); + if (filter.m_boundsShape) + { + EXPECT_EQ(filter.m_boundsShape->GetShapeType(), Physics::ShapeType::Sphere); + Physics::SphereShapeConfiguration* sphereShape = + 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_boundsPose.GetTranslation(); + EXPECT_EQ(loc.GetX(), point1.GetX()); + EXPECT_EQ(loc.GetY(), point1.GetY()); + EXPECT_EQ(loc.GetZ(), point1.GetZ()); + + EXPECT_EQ(filter.m_nameFilter, "FooBarFilter"); + return AZ::Success(EntityNameList{ "FooBar" }); + })); + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + 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::msg::Result::RESULT_OK); + } + + //! Try to call the service with invalid data (too few points) for the sphere shape + TEST_F(SimulationInterfaceROS2TestFixture, GetEntitiesWithShapeSphereWithInvalidData) + { + using ::testing::_; + auto node = GetRos2Node(); + [[maybe_unused]] testing::StrictMock mock; + auto client = node->create_client("/get_entities"); + auto request = std::make_shared(); + request->filters.bounds.points.resize(1); // too few points + request->filters.bounds.type = simulation_interfaces::msg::Bounds::TYPE_SPHERE; + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; + auto response = future.get(); + EXPECT_NE(response->result.result, simulation_interfaces::msg::Result::RESULT_OK); + } + + //! Happy path test for GetEntities with a box shape + TEST_F(SimulationInterfaceROS2TestFixture, GetEntitiesWithShapeBox) + { + using ::testing::_; + auto node = GetRos2Node(); + testing::NiceMock mock; + auto client = node->create_client("/get_entities"); + auto request = std::make_shared(); + request->filters.bounds.points.resize(2); + + const AZ::Vector3 point1(1.0f, 2.0f, 3.0f); + const AZ::Vector3 dims(4.0f, 5.0f, 6.0f); + const AZ::Vector3 point2 = point1 + dims; + + request->filters.bounds.points[0].x = point1.GetX(); + request->filters.bounds.points[0].y = point1.GetY(); + request->filters.bounds.points[0].z = point1.GetZ(); + request->filters.bounds.points[1].x = point2.GetX(); + request->filters.bounds.points[1].y = point2.GetY(); + request->filters.bounds.points[1].z = point2.GetZ(); + request->filters.bounds.type = simulation_interfaces::msg::Bounds::TYPE_BOX; + + EXPECT_CALL(mock, GetEntities(_)) + .WillOnce(::testing::Invoke( + [=](const EntityFilters& filter) + { + EXPECT_NE(filter.m_boundsShape, nullptr); + if (filter.m_boundsShape) + { + EXPECT_EQ(filter.m_boundsShape->GetShapeType(), Physics::ShapeType::Box); + Physics::BoxShapeConfiguration* boxShape = + 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_boundsPose.GetTranslation(); + EXPECT_EQ(loc, AZ::Vector3::CreateZero()); + return AZ::Success(EntityNameList{ "FooBar" }); + })); + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + 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::msg::Result::RESULT_OK); + } + + //! Try to call the service with invalid data (first point is greater than second point in box) + TEST_F(SimulationInterfaceROS2TestFixture, GetEntitiesWithShapeBoxInvalid) + { + using ::testing::_; + auto node = GetRos2Node(); + + // strict mock, since we don't want to call the real implementation in this test + [[maybe_unused]] testing::StrictMock mock; + + auto client = node->create_client("/get_entities"); + auto request = std::make_shared(); + request->filters.bounds.points.resize(2); + + request->filters.bounds.points[0].x = 2.0f; + request->filters.bounds.points[0].y = 3.0f; + request->filters.bounds.points[0].z = 4.0f; + request->filters.bounds.points[1].x = -2.0f; + request->filters.bounds.points[1].y = -3.0f; + request->filters.bounds.points[1].z = 5.0f; + request->filters.bounds.type = simulation_interfaces::msg::Bounds::TYPE_BOX; + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; + auto response = future.get(); + EXPECT_NE(response->result.result, simulation_interfaces::msg::Result::RESULT_OK); + } + + //! check if capabilities vector is empty when no features are provided by SimulationInterfaces gem + TEST_F(SimulationInterfaceROS2TestFixture, GetSimulationFeaturesNoFeaturesProvidedByGem) + { + using ::testing::_; + auto node = GetRos2Node(); + + auto client = node->create_client("/get_simulation_features"); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; + auto response = future.get(); + EXPECT_TRUE(response->features.features.empty()) << "Features vector is not empty."; + } + + //! check if features are returned when the feature is provided by SimulationFeaturesAggregator + //! and the feature is not in the list of features provided by the gem + TEST_F(SimulationInterfaceROS2TestFixture, GetSimulationFeaturesSomeFeaturesProvided) + { + SimulationFeaturesAggregatorRequestsMockedHandler mockAggregator; + using ::testing::_; + auto node = GetRos2Node(); + 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 + }; + EXPECT_CALL(mockAggregator, GetSimulationFeatures()) + .WillOnce(::testing::Invoke( + [&](void) + { + return features; + })); + + auto client = node->create_client("/get_simulation_features"); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; + auto response = future.get(); + ASSERT_FALSE(response->features.features.empty()) << "Features vector is empty."; + EXPECT_EQ(response->features.features.size(), 1) << "Features vector should contain one feature only."; + EXPECT_EQ(response->features.features[0], simulation_interfaces::msg::SimulatorFeatures::SPAWNING) << "Feature is not SPAWNING."; + } + + //! Check if service succeeds when the name is valid + TEST_F(SimulationInterfaceROS2TestFixture, TryToSpawnWithValidName) + { + using ::testing::_; + SimulationEntityManagerMockedHandler mock; + + auto node = GetRos2Node(); + auto client = node->create_client("/spawn_entity"); + auto request = std::make_shared(); + request->name = "valid_name"; + request->uri = "test_uri"; + request->entity_namespace = "test_namespace"; + 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_EQ(name, "valid_name"); + EXPECT_EQ(uri, "test_uri"); + EXPECT_EQ(entityNamespace, "test_namespace"); + EXPECT_TRUE(allowRename); + completedCb(AZ::Success("valid_name")); + })); + SpinAppUntilFuture(future); + + 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::msg::Result::RESULT_OK); + } + + //! Check if service fails when the name is invalid + TEST_F(SimulationInterfaceROS2TestFixture, TryToSpawnWithInvalidName) + { + // strict mock, since we don't want to call the real implementation in this test + [[maybe_unused]] testing::StrictMock mock; + + auto node = GetRos2Node(); + auto client = node->create_client("/spawn_entity"); + auto request = std::make_shared(); + request->name = "invalid name"; // invalid name + request->uri = "test_uri"; + request->entity_namespace = "test_namespace"; + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + 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."; + } + + //! Check if service fails when the namespace is invalid + TEST_F(SimulationInterfaceROS2TestFixture, TryToSpawnWithInvalidNamespace) + { + // strict mock, since we don't want to call the real implementation in this test + [[maybe_unused]] testing::StrictMock mock; + + auto node = GetRos2Node(); + auto client = node->create_client("/spawn_entity"); + auto request = std::make_shared(); + request->name = "valid_name"; + request->uri = "test_uri"; + request->entity_namespace = "invalid namespace"; + + auto future = client->async_send_request(request); + SpinAppUntilFuture(future); + + 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."; + } + + TEST_F(SimulationInterfaceROS2TestFixture, SimulationStepsSuccess) + { + using ::testing::_; + auto node = GetRos2Node(); + SimulationManagerMockedHandler mock; + auto client = rclcpp_action::create_client(node, "/simulate_steps"); + auto goal = std::make_shared(); + constexpr AZ::u64 steps = 10; + goal->steps = steps; + + ASSERT_TRUE(client->wait_for_action_server(std::chrono::seconds(0)) == true) << "Action server is unavailable"; + + EXPECT_CALL(mock, IsSimulationPaused()) + .WillOnce(::testing::Invoke( + []() + { + return true; + })); + + EXPECT_CALL(mock, StepSimulation(steps)); + + EXPECT_CALL(mock, IsSimulationStepsActive()) + .WillOnce(::testing::Invoke( + []() + { + return false; + })); + + auto future = client->async_send_goal(*goal); + SpinAppUntilFuture(future); + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + auto goalHandle = future.get(); + ASSERT_NE(goalHandle, nullptr); + auto result = client->async_get_result(goalHandle); + SpinAppUntilFuture(result); + ASSERT_TRUE(result.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + EXPECT_EQ(result.get().result->result.result, simulation_interfaces::msg::Result::RESULT_OK); + } + + TEST_F(SimulationInterfaceROS2TestFixture, SimulationStepsFailureSimulationIsNotPaused) + { + using ::testing::_; + auto node = GetRos2Node(); + SimulationManagerMockedHandler mock; + auto client = rclcpp_action::create_client(node, "/simulate_steps"); + auto goal = std::make_shared(); + goal->steps = 10; + + ASSERT_TRUE(client->wait_for_action_server(std::chrono::seconds(0)) == true) << "Action server is unavailable"; + + EXPECT_CALL(mock, IsSimulationPaused()) + .WillOnce(::testing::Invoke( + []() + { + return false; + })); + + auto future = client->async_send_goal(*goal); + SpinAppUntilFuture(future); + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + auto goalHandle = future.get(); + ASSERT_NE(goalHandle, nullptr); + auto result = client->async_get_result(goalHandle); + SpinAppUntilFuture(result); + ASSERT_TRUE(result.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + EXPECT_EQ(result.get().result->result.result, simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED); + } + + TEST_F(SimulationInterfaceROS2TestFixture, SimulationStepsCancelled) + { + using ::testing::_; + auto node = GetRos2Node(); + SimulationManagerMockedHandler mock; + auto client = rclcpp_action::create_client(node, "/simulate_steps"); + auto goal = std::make_shared(); + constexpr AZ::u64 steps = 10; + goal->steps = steps; + + ASSERT_TRUE(client->wait_for_action_server(std::chrono::seconds(0)) == true) << "Action server is unavailable"; + + EXPECT_CALL(mock, IsSimulationPaused()) + .WillOnce(::testing::Invoke( + []() + { + return true; + })); + + EXPECT_CALL(mock, StepSimulation(steps)); + + EXPECT_CALL(mock, IsSimulationStepsActive()) + .WillRepeatedly(::testing::Invoke( + []() + { + return true; + })); + + EXPECT_CALL(mock, CancelStepSimulation()); + + auto future = client->async_send_goal(*goal); + SpinAppUntilFuture(future); + ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + auto goalHandle = future.get(); + ASSERT_NE(goalHandle, nullptr); + auto cancelFuture = client->async_cancel_goal(goalHandle); + SpinAppUntilFuture(cancelFuture); + ASSERT_TRUE(cancelFuture.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + } + +} // namespace UnitTest + +// required to support running integration tests with Qt and PhysX +AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + AzQtComponents::PrepareQtPaths(); + QApplication app(argc, argv); + AZ::Test::printUnusedParametersWarning(argc, argv); + AZ::Test::addTestEnvironments({ new UnitTest::SimulationInterfaceROS2TestEnvironment() }); + int result = RUN_ALL_TESTS(); + return result; +} + +IMPLEMENT_TEST_EXECUTABLE_MAIN(); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h new file mode 100644 index 0000000000..336981c295 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h @@ -0,0 +1,38 @@ +#pragma once +#include +#include +#include +namespace UnitTest +{ + using namespace SimulationInterfaces; + class SimulationEntityManagerMockedHandler : public SimulationInterfaces::SimulationEntityManagerRequestBus::Handler + { + public: + SimulationEntityManagerMockedHandler() + { + SimulationInterfaces::SimulationEntityManagerRequestBus::Handler::BusConnect(); + } + ~SimulationEntityManagerMockedHandler() + { + SimulationInterfaces::SimulationEntityManagerRequestBus::Handler::BusDisconnect(); + } + + MOCK_METHOD1(GetEntities, AZ::Outcome(const EntityFilters& filter)); + MOCK_METHOD1(GetEntityState, AZ::Outcome(const AZStd::string& name)); + MOCK_METHOD1(GetEntitiesStates, AZ::Outcome(const EntityFilters& filter)); + MOCK_METHOD2(SetEntityState, AZ::Outcome(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()); + MOCK_METHOD0(ResetAllEntitiesToInitialState, void()); + MOCK_METHOD6( + SpawnEntity, + void( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb)); + }; +} // namespace UnitTest diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h new file mode 100644 index 0000000000..7c7f867d55 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h @@ -0,0 +1,24 @@ +#pragma once +#include +#include +#include +namespace UnitTest +{ + using namespace SimulationInterfaces; + class SimulationFeaturesAggregatorRequestsMockedHandler : public SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::Handler + { + public: + SimulationFeaturesAggregatorRequestsMockedHandler() + { + SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::Handler::BusConnect(); + } + ~SimulationFeaturesAggregatorRequestsMockedHandler() + { + SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::Handler::BusDisconnect(); + } + + 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/Mocks/SimulationManagerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationManagerMock.h new file mode 100644 index 0000000000..475178de97 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationManagerMock.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +namespace UnitTest +{ + using namespace SimulationInterfaces; + class SimulationManagerMockedHandler : public SimulationInterfaces::SimulationManagerRequestBus::Handler + { + public: + SimulationManagerMockedHandler() + { + SimulationInterfaces::SimulationManagerRequestBus::Handler::BusConnect(); + } + ~SimulationManagerMockedHandler() + { + SimulationInterfaces::SimulationManagerRequestBus::Handler::BusDisconnect(); + } + + MOCK_METHOD1(ReloadLevel, void(ReloadLevelCallback)); + MOCK_METHOD1(SetSimulationPaused, void(bool)); + MOCK_METHOD1(StepSimulation, void(AZ::u64)); + MOCK_METHOD(bool, IsSimulationPaused, (), (const)); + MOCK_METHOD0(CancelStepSimulation, void()); + MOCK_METHOD(bool, IsSimulationStepsActive, (), (const)); + MOCK_METHOD(SimulationState, GetSimulationState, (), (const)); + MOCK_METHOD1(SetSimulationState, AZ::Outcome(SimulationState)); + }; +} // namespace UnitTest diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp new file mode 100644 index 0000000000..50c07eb88f --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp @@ -0,0 +1,204 @@ + +/* + * 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 "TestFixture.h" +#include +namespace UnitTest +{ + + TEST_F(SimulationInterfaceTestFixture, EmptyScene) + { + using namespace SimulationInterfaces; + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities.IsSuccess()); + EXPECT_EQ(entities.GetValue().size(), 0); + } + + TEST_F(SimulationInterfaceTestFixture, AddSimulatedEntityThenRemove) + { + using namespace SimulationInterfaces; + const AZ::EntityId entityId1 = CreateEntityWithStaticBodyComponent("Foo", AZ::Transform::CreateIdentity()); + const AZ::EntityId entityId2 = CreateEntityWithStaticBodyComponent("Bar", AZ::Transform::CreateIdentity()); + + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities.IsSuccess()); + ASSERT_EQ(entities.GetValue().size(), 2); + DeleteEntity(entityId1); + + AZ::Outcome entities2; + SimulationEntityManagerRequestBus::BroadcastResult( + entities2, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities2.IsSuccess()); + EXPECT_EQ(entities2.GetValue().size(), 1); + + DeleteEntity(entityId2); + AZ::Outcome entities3; + SimulationEntityManagerRequestBus::BroadcastResult( + entities3, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities3.IsSuccess()); + EXPECT_EQ(entities3.GetValue().size(), 0); + } + + TEST_F(SimulationInterfaceTestFixture, AddEntitiesWithDupName) + { + using namespace SimulationInterfaces; + + const AZ::EntityId entityId1 = CreateEntityWithStaticBodyComponent("Bar1", AZ::Transform::CreateIdentity()); + const AZ::EntityId entityId2 = CreateEntityWithStaticBodyComponent("Bar1", AZ::Transform::CreateIdentity()); + + AZ::Outcome entities; + + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities.IsSuccess()); + EXPECT_EQ(entities.GetValue().size(), 2); + EXPECT_NE(entities.GetValue()[0], entities.GetValue()[1]); + DeleteEntity(entityId1); + DeleteEntity(entityId2); + } + + TEST_F(SimulationInterfaceTestFixture, TestShapeFilter) + { + // This test is disabled since due to some issue outside to this gem, the rigid body is created without the collider shape + // and the filter is not applied. This test will be enabled once the issue is resolved. + GTEST_SKIP() << "Need to fix the issue with the collider shape creation."; + using namespace SimulationInterfaces; + const AZ::EntityId entityId1 = + CreateEntityWithStaticBodyComponent("Inside", AZ::Transform::CreateTranslation(AZ::Vector3(0.0f, 0.0f, 0.0f))); + const AZ::EntityId entityId2 = + CreateEntityWithStaticBodyComponent("Outside", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); + + EntityFilters filter; + filter.m_boundsShape = AZStd::make_shared(2.0f); + + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + auto* physicsSystem = AZ::Interface::Get(); + physicsSystem->Simulate(1.0f / 60.0f); + + ASSERT_TRUE(entities.IsSuccess()); + ASSERT_EQ(entities.GetValue().size(), 1); + EXPECT_EQ(entities.GetValue().front(), "Inside"); + + DeleteEntity(entityId1); + DeleteEntity(entityId2); + } + + TEST_F(SimulationInterfaceTestFixture, TestRegexFilter) + { + using namespace SimulationInterfaces; + const AZ::EntityId entityId1 = + CreateEntityWithStaticBodyComponent("WillMatch", AZ::Transform::CreateTranslation(AZ::Vector3(0.0f, 0.0f, 0.0f))); + const AZ::EntityId entityId2 = + CreateEntityWithStaticBodyComponent("WontMatch", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); + + EntityFilters filter; + filter.m_nameFilter = "Will.*"; + + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + + ASSERT_TRUE(entities.IsSuccess()); + ASSERT_EQ(entities.GetValue().size(), 1); + EXPECT_EQ(entities.GetValue().front(), "WillMatch"); + + DeleteEntity(entityId1); + DeleteEntity(entityId2); + } + + TEST_F(SimulationInterfaceTestFixture, TestRegexFilterInvalid) + { + // Invalid regex should not match any entity + using namespace SimulationInterfaces; + const AZ::EntityId entityId1 = + CreateEntityWithStaticBodyComponent("WillMatch", AZ::Transform::CreateTranslation(AZ::Vector3(0.0f, 0.0f, 0.0f))); + const AZ::EntityId entityId2 = + CreateEntityWithStaticBodyComponent("WontMatch", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); + + EntityFilters filter; + filter.m_nameFilter = "[a-z"; + + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + + EXPECT_FALSE(entities.IsSuccess()); + + DeleteEntity(entityId1); + DeleteEntity(entityId2); + } + + TEST_F(SimulationInterfaceTestFixture, SmokeTestGetEntityState) + { + // Invalid regex should not match any entity + using namespace SimulationInterfaces; + const AZStd::string entityName = "DroppedBall"; + const AZ::EntityId entityId1 = + CreateEntityWithStaticBodyComponent(entityName, AZ::Transform::CreateTranslation(AZ::Vector3(2.0f, 0.0f, 10.0f))); + + EntityFilters filter; + AZ::Outcome stateBeforeResult; + SimulationEntityManagerRequestBus::BroadcastResult( + stateBeforeResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + EXPECT_TRUE(stateBeforeResult.IsSuccess()); + const auto& stateBefore = stateBeforeResult.GetValue(); + EXPECT_EQ(stateBefore.m_pose.GetTranslation(), AZ::Vector3(2.0f, 0.0f, 10.0f)); + for (int i = 0; i < 10; i++) + { + auto* physicsSystem = AZ::Interface::Get(); + physicsSystem->Simulate(1.0f / 60.0f); + } + AZ::Outcome stateAfterResult; + SimulationEntityManagerRequestBus::BroadcastResult( + stateAfterResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + EXPECT_TRUE(stateAfterResult.IsSuccess()); + const auto& stateAfter = stateAfterResult.GetValue(); + AZ::Vector3 deltaPos = stateAfter.m_pose.GetTranslation() - stateBefore.m_pose.GetTranslation(); + + // check if the entity moved + EXPECT_GT(deltaPos.GetLength(), 0.0f); + + // check if entity has velocity + EXPECT_GT(stateAfter.m_twistLinear.GetLength(), 0.0f); + + DeleteEntity(entityId1); + } + + TEST_F(SimulationInterfaceTestFixture, TestEntitySpawnedDeletionEmptyScene) + { + using namespace SimulationInterfaces; + bool deletionWasCompleted = false; + DeletionCompletedCb deleteAllCompletion = [&deletionWasCompleted](const AZ::Outcome& 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 +AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + AzQtComponents::PrepareQtPaths(); + QApplication app(argc, argv); + AZ::Test::printUnusedParametersWarning(argc, argv); + AZ::Test::addTestEnvironments({ new UnitTest::SimulationInterfaceTestEnvironment() }); + int result = RUN_ALL_TESTS(); + return result; +} + +IMPLEMENT_TEST_EXECUTABLE_MAIN(); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp new file mode 100644 index 0000000000..28233734e4 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -0,0 +1,212 @@ + +/* + * 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 "TestFixture.h" +#include + +namespace UnitTest +{ + class SimulationInterfaceTestEnvironmentWithAssets : public SimulationInterfaceTestEnvironment + { + protected: + void PostSystemEntityActivate(); + }; + + void SimulationInterfaceTestEnvironmentWithAssets::PostSystemEntityActivate() + { + // Prepare the asset catalog and ensure that our test asset (testsimulationentity.spawnable) is loaded and + // ready to be used in test scenarios. + AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize); + + AZ::ComponentApplication* app = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(app, &AZ::ComponentApplicationBus::Events::GetApplication); + AZ_Assert(app, "Failed to get application"); + auto products = AZ::Utils::GetProjectProductPathForPlatform().c_str(); + AZ::IO::Path assetCatalogPath = AZ::IO::Path(products) / "assetcatalog.xml"; + bool catalogExists = AZ::IO::FileIOBase::GetInstance()->Exists(assetCatalogPath.c_str()); + AZ_Assert(catalogExists, "Asset Catalog in %s does not exist", assetCatalogPath.c_str()); + + AZ::Data::AssetCatalogRequestBus::Broadcast(&AZ::Data::AssetCatalogRequestBus::Events::LoadCatalog, assetCatalogPath.c_str()); + + const AZ::IO::Path TestSpawnable = "sampleasset/testsimulationentity.spawnable"; + const AZ::IO::Path TestSpawnableGlobalPath = AZ::IO::Path(products) / TestSpawnable; + bool spawnableExists = AZ::IO::FileIOBase::GetInstance()->Exists(assetCatalogPath.c_str()); + AZ_Assert(spawnableExists, "%s does not exist", TestSpawnableGlobalPath.c_str()); + + AZ::Data::AssetId assetId; + AZ::Data::AssetCatalogRequestBus::BroadcastResult( + assetId, + &AZ::Data::AssetCatalogRequestBus::Events::GetAssetIdByPath, + TestSpawnable.c_str(), + AZ::Data::s_invalidAssetType, + false); + AZ_Assert(assetId.IsValid(), "Failed to get asset id for %s", TestSpawnable.c_str()); + } + + int getNumberOfEntities() + { + using namespace SimulationInterfaces; + AZ::Outcome 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. + // It has prepared asset catalog, and we are able to spawn entities with the test asset. + + using namespace SimulationInterfaces; + constexpr AZStd::string_view entityName = "MySuperDuperEntity"; + const AZ::Transform initialPose = AZ::Transform::CreateTranslation(AZ::Vector3(0.0f, 0.0f, 0.0f)); + constexpr AZStd::string_view uri = "product_asset:///sampleasset/testsimulationentity.spawnable"; + constexpr AZStd::string_view entityNamespace = ""; + AZStd::atomic_bool completed = false; + SpawnCompletedCb completedCb = [&](const AZ::Outcome& result) + { + EXPECT_TRUE(result.IsSuccess()); + completed = true; + }; + + 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); + + // try to spawn entity with the same name, expect failure + AZStd::atomic_bool completed2 = false; + SpawnCompletedCb failedSpawnCompletedCb = [&](const AZ::Outcome& result) + { + EXPECT_FALSE(result.IsSuccess()); + completed2 = true; + }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, + entityName, + uri, + entityNamespace, + initialPose, + allowRename, + failedSpawnCompletedCb); + EXPECT_TRUE(completed2); + + // list simulation entities + AZ::Outcome entitiesResult; + SimulationEntityManagerRequestBus::BroadcastResult( + entitiesResult, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entitiesResult.IsSuccess()); + const auto& entities = entitiesResult.GetValue(); + EXPECT_EQ(entities.size(), 1); + + ASSERT_FALSE(entities.empty()) << "Simulated Entities Empty"; + const AZStd::string spawnedEntityName = entities.front(); + printf("Spawned entity name %s\n", spawnedEntityName.c_str()); + + // run physics simulation + StepPhysics(100); + + // Get entity state, + AZ::Outcome entityStatesResult; + SimulationEntityManagerRequestBus::BroadcastResult( + entityStatesResult, &SimulationEntityManagerRequestBus::Events::GetEntitiesStates, EntityFilters()); + ASSERT_TRUE(entityStatesResult.IsSuccess()); + + const auto& entityStates = entityStatesResult.GetValue(); + auto entityState = entityStates.find(spawnedEntityName); + ASSERT_NE(entityState, entityStates.end()); + EXPECT_EQ(entityState->first, spawnedEntityName); + + // check if the entity moved + EXPECT_GE(entityState->second.m_pose.GetTranslation().GetDistance(initialPose.GetTranslation()), 1.0f); + + // set new entity state - move the entity to X=1000 meters + const AZ::Vector3 newPosition = AZ::Vector3(1000.0f, 0.0f, 0.0f); + const EntityState newState = { AZ::Transform::CreateTranslation(newPosition), + AZ::Vector3::CreateZero(), + AZ::Vector3::CreateZero() }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SetEntityState, spawnedEntityName, newState); + + StepPhysics(); + + // 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_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); + ASSERT_TRUE(entitiesFiltered.IsSuccess()); + EXPECT_EQ(entitiesFiltered.GetValue().size(), 1); + + // delete entity using its name + DeletionCompletedCb deletionCompletedCb = [](const AZ::Outcome& result) + { + EXPECT_TRUE(result.IsSuccess()); + }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, deletionCompletedCb); + TickApp(100); + + // 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& result) + { + }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity1", uri, entityNamespace, initialPose, false, cb); + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity2", uri, entityNamespace, initialPose, false, cb); + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity3", uri, entityNamespace, initialPose, false, cb); + TickApp(100); + EXPECT_EQ(getNumberOfEntities(), 3); + + // delete all entities + bool deletionWasCompleted = false; + DeletionCompletedCb deleteAllCompletion = [&deletionWasCompleted](const AZ::Outcome& result) + { + deletionWasCompleted = true; + EXPECT_TRUE(result.IsSuccess()); + }; + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); + + TickApp(100); + EXPECT_TRUE(deletionWasCompleted); + EXPECT_EQ(getNumberOfEntities(), 0); + } + +} // namespace UnitTest + +// required to support running integration tests with Qt and PhysX +AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + AzQtComponents::PrepareQtPaths(); + QApplication app(argc, argv); + AZ::Test::printUnusedParametersWarning(argc, argv); + AZ::Test::addTestEnvironments({ new UnitTest::SimulationInterfaceTestEnvironmentWithAssets() }); + int result = RUN_ALL_TESTS(); + return result; +} + +IMPLEMENT_TEST_EXECUTABLE_MAIN(); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp new file mode 100644 index 0000000000..4dd1772bd9 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp @@ -0,0 +1,150 @@ + +/* + * 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 "TestFixture.h" +#include "Clients/SimulationEntitiesManager.h" +#include +namespace UnitTest +{ + void SimulationInterfaceTestEnvironment::AddGemsAndComponents() + { + constexpr AZStd::array requiredGems = { "PhysX5", // required for PhysX Dynamic + "LmbrCentral", // for shapes + "SimulationInterfaces" }; + AddActiveGems(requiredGems); + AddDynamicModulePaths({ "PhysX5.Gem" }); + AddDynamicModulePaths({ "LmbrCentral" }); + AddComponentDescriptors({ + SimulationInterfaces::SimulationEntitiesManager::CreateDescriptor(), + }); + AddRequiredComponents({ SimulationInterfaces::SimulationEntitiesManager::TYPEINFO_Uuid() }); + } + + void SimulationInterfaceTestEnvironment::PostSystemEntityActivate() + { + AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize); + + // load asset catalog + } + + AZ::ComponentApplication* SimulationInterfaceTestEnvironment::CreateApplicationInstance() + { + // Using ToolsTestApplication to have AzFramework and AzToolsFramework components. + return aznew UnitTest::ToolsTestApplication("SimulationInterfaceTestEnvironment"); + } + + void SimulationInterfaceTestFixture::AddAsset(const AZStd::string& assetPath) + { + AZ::Data::AssetInfo info; + info.m_relativePath = assetPath; + info.m_sizeBytes = 1; + AZ::Data::AssetId id = AZ::Data::AssetId(AZ::Uuid::CreateRandom()); + AZ::Data::AssetCatalogRequestBus::Broadcast(&AZ::Data::AssetCatalogRequestBus::Events::RegisterAsset, id, info); + m_registeredAssets.insert(id); + } + + AzPhysics::SceneHandle SimulationInterfaceTestFixture::GetDefaultSceneHandle() const + { + return m_testSceneHandle; + } + + AZ::EntityId SimulationInterfaceTestFixture::CreateEntityWithStaticBodyComponent( + const AZStd::string& entityName, const AZ::Transform& transform) + { + AZStd::unique_ptr entity = AZStd::make_unique(entityName.c_str()); + auto* transformComponent = entity->CreateComponent(AZ::TransformComponentTypeId); + AZ_Assert(transformComponent, "Failed to create TransformComponent"); + auto* transformInterface = azrtti_cast(transformComponent); + AZ_Assert(transformInterface, "Failed to get TransformInterface"); + transformInterface->SetWorldTM(transform); + entity->CreateComponent(AZ::Uuid(PhysXRigidBodyComponentTypeId)); + entity->CreateComponent(AZ::Uuid(PhysXShapeColliderComponentTypeId)); + entity->CreateComponent(AZ::Uuid(SphereShapeComponentTypeId)); + entity->Init(); + entity->Activate(); + AZ_Assert(entity->GetState() == AZ::Entity::State::Active, "Entity is not active"); + + auto id = entity->GetId(); + m_entities.emplace(AZStd::make_pair(id, AZStd::move(entity))); + + return id; + } + + void SimulationInterfaceTestFixture::ClearEntities() + { + for (auto& entity : m_entities) + { + entity.second->Deactivate(); + } + m_entities.clear(); + } + void SimulationInterfaceTestFixture::DeleteEntity(const AZ::EntityId& entityId) + { + auto findIt = m_entities.find(entityId); + if (findIt != m_entities.end()) + { + findIt->second->Deactivate(); + m_entities.erase(findIt); + } + } + + void SimulationInterfaceTestFixture::SetUp() + { + if (auto* physicsSystem = AZ::Interface::Get()) + { + AzPhysics::SceneConfiguration sceneConfiguration = physicsSystem->GetDefaultSceneConfiguration(); + sceneConfiguration.m_sceneName = AzPhysics::DefaultPhysicsSceneName; + m_testSceneHandle = physicsSystem->AddScene(sceneConfiguration); + m_defaultScene = physicsSystem->GetScene(m_testSceneHandle); + } + + Physics::DefaultWorldBus::Handler::BusConnect(); + } + + void SimulationInterfaceTestFixture::TearDown() + { + ClearEntities(); + Physics::DefaultWorldBus::Handler::BusDisconnect(); + m_defaultScene = nullptr; + + // Clean up the Test scene + if (auto* physicsSystem = AZ::Interface::Get()) + { + physicsSystem->RemoveScene(m_testSceneHandle); + } + m_testSceneHandle = AzPhysics::InvalidSceneHandle; + + for (const auto& id : m_registeredAssets) + { + AZ::Data::AssetCatalogRequestBus::Broadcast(&AZ::Data::AssetCatalogRequestBus::Events::UnregisterAsset, id); + } + m_registeredAssets.clear(); + } + + void SimulationInterfaceTestFixture::StepPhysics(int numSteps) + { + for (int i = 0; i < numSteps; i++) + { + auto* physicsSystem = AZ::Interface::Get(); + physicsSystem->Simulate(1.0f / 60.0f); + } + } + + void SimulationInterfaceTestFixture::TickApp(int numTicks) + { + AZ::ComponentApplication* app = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(app, &AZ::ComponentApplicationBus::Events::GetApplication); + AZ_Assert(app, "Failed to get application"); + for (int i = 0; i < numTicks; i++) + { + app->Tick(); + } + } + +} // namespace UnitTest \ No newline at end of file diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h new file mode 100644 index 0000000000..4eb85efdf2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h @@ -0,0 +1,95 @@ + +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace UnitTest +{ + class SimulationInterfaceTestEnvironment : public AZ::Test::GemTestEnvironment + { + // AZ::Test::GemTestEnvironment overrides ... + void AddGemsAndComponents() override; + AZ::ComponentApplication* CreateApplicationInstance() override; + + protected: + void PostSystemEntityActivate() override; + + public: + SimulationInterfaceTestEnvironment() = default; + ~SimulationInterfaceTestEnvironment() override = default; + }; + + class SimulationInterfaceTestFixture + : public ::testing::Test + , protected Physics::DefaultWorldBus::Handler + { + protected: + constexpr static auto PhysXRigidBodyComponentTypeId = "{D4E52A70-BDE1-4819-BD3C-93AB3F4F3BE3}"; // From PhysX + constexpr static auto PhysXStaticBodyComponentTypeId = "{A2CCCD3D-FB31-4D65-8DCD-2CD7E1D09538}"; // From PhysX + constexpr static auto PhysXShapeColliderComponentTypeId = "{30CC9E77-378C-49DF-9617-6BF191901FE0}"; // From PhysX + constexpr static auto PhysXSphereColliderComponentTypeId = "{108CD341-E5C3-4AE1-B712-21E81ED6C277}"; // From PhysX + constexpr static auto SphereShapeComponentTypeId = "{E24CBFF0-2531-4F8D-A8AB-47AF4D54BCD2}"; // From LmbrCentral + + void SetUp() override; + void TearDown() override; + + AZ::EntityId CreateEntityWithStaticBodyComponent(const AZStd::string& entityName, const AZ::Transform& transform); + + void DeleteEntity(const AZ::EntityId& entityId); + void ClearEntities(); + + AZStd::unordered_map> m_entities; + + void AddAsset(const AZStd::string& assetPath); + + //! Ask the physics system to step forward in time + void StepPhysics(int numSteps = 1); + + //! Ask the application to tick forward in time + void TickApp(int numTicks = 1); + + private: + AzPhysics::SceneHandle GetDefaultSceneHandle() const override; + AzPhysics::Scene* m_defaultScene = nullptr; + AzPhysics::SceneHandle m_testSceneHandle = AzPhysics::InvalidSceneHandle; + AZStd::unordered_set m_registeredAssets; + }; +} // namespace UnitTest \ No newline at end of file diff --git a/Gems/SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake b/Gems/SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake new file mode 100644 index 0000000000..6e7974cb19 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake @@ -0,0 +1,12 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Tests/Tools/Mocks/SimulationManagerMock.h + Tests/Tools/Mocks/SimulationEntityManagerMock.h + Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h + Tests/Tools/InterfacesTest.cpp +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..c87b6436a1 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake @@ -0,0 +1,16 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Include/SimulationInterfaces/Result.h + Include/SimulationInterfaces/TagFilter.h + Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h + Include/SimulationInterfaces/SimulationInterfacesTypeIds.h + Include/SimulationInterfaces/SimulationMangerRequestBus.h + Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h + Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h + Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_api_files.cmake new file mode 100644 index 0000000000..263b8b77c4 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_api_files.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + + +set(FILES +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_app_test.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_app_test.cmake new file mode 100644 index 0000000000..a2152427e2 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_app_test.cmake @@ -0,0 +1,11 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Tests/Tools/TestFixture.cpp + Tests/Tools/TestFixture.h + Tests/Tools/SimulationIterfaceAppTest.cpp +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake new file mode 100644 index 0000000000..4fe290c118 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake @@ -0,0 +1,16 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Source/Tools/SimulationManagerEditor.cpp + Source/Tools/SimulationManagerEditor.h + Source/Tools/SimulationEntitiesManagerEditor.cpp + Source/Tools/SimulationEntitiesManagerEditor.h + Source/Tools/SimulationFeaturesAggregatorEditor.cpp + Source/Tools/SimulationFeaturesAggregatorEditor.h + Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp + Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_shared_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_shared_files.cmake new file mode 100644 index 0000000000..817cc1b292 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_shared_files.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Source/Tools/SimulationInterfacesEditorModule.cpp +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_tests_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_tests_files.cmake new file mode 100644 index 0000000000..c5ed58dbdc --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_tests_files.cmake @@ -0,0 +1,11 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Tests/Tools/TestFixture.cpp + Tests/Tools/TestFixture.h + Tests/Tools/SimulationInterfaceTests.cpp +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake new file mode 100644 index 0000000000..52f280a259 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake @@ -0,0 +1,56 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Source/SimulationInterfacesModuleInterface.cpp + Source/SimulationInterfacesModuleInterface.h + Source/Clients/SimulationManager.cpp + Source/Clients/SimulationManager.h + Source/Clients/SimulationEntitiesManager.cpp + Source/Clients/SimulationEntitiesManager.h + Source/Clients/CommonUtilities.cpp + Source/Clients/CommonUtilities.h + Source/Clients/SimulationFeaturesAggregator.cpp + Source/Clients/SimulationFeaturesAggregator.h + Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp + Source/Clients/ROS2SimulationInterfacesSystemComponent.h + Source/Interfaces/IROS2HandlerBase.h + Source/Actions/SimulateStepsActionServerHandler.cpp + Source/Actions/SimulateStepsActionServerHandler.h + Source/Actions/ROS2ActionBase.h + Source/Services/ROS2ServiceBase.h + Source/Services/GetEntitiesServiceHandler.cpp + Source/Services/GetEntitiesServiceHandler.h + Source/Services/DeleteEntityServiceHandler.cpp + Source/Services/DeleteEntityServiceHandler.h + Source/Services/GetEntitiesServiceHandler.cpp + Source/Services/GetEntitiesServiceHandler.h + Source/Services/GetEntitiesStatesServiceHandler.cpp + Source/Services/GetEntitiesStatesServiceHandler.h + Source/Services/GetEntityStateServiceHandler.cpp + Source/Services/GetEntityStateServiceHandler.h + Source/Services/DeleteEntityServiceHandler.cpp + Source/Services/DeleteEntityServiceHandler.h + Source/Services/SetEntityStateServiceHandler.cpp + Source/Services/SetEntityStateServiceHandler.h + Source/Services/GetSpawnablesServiceHandler.cpp + 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/Services/SetSimulationStateServiceHandler.cpp + Source/Services/SetSimulationStateServiceHandler.h + Source/Services/GetSimulationStateServiceHandler.cpp + Source/Services/GetSimulationStateServiceHandler.h + Source/Services/StepSimulationServiceHandler.cpp + Source/Services/StepSimulationServiceHandler.h + Source/Utils/RegistryUtils.cpp + Source/Utils/RegistryUtils.h + Source/Utils/Utils.h +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_shared_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_shared_files.cmake new file mode 100644 index 0000000000..a5659e6feb --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_shared_files.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Source/Clients/SimulationInterfacesModule.cpp +) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_tests_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_tests_files.cmake new file mode 100644 index 0000000000..e12a6decea --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_tests_files.cmake @@ -0,0 +1,9 @@ +# Copyright (c) Contributors to the Open 3D Engine Project. +# For complete copyright and license terms please see the LICENSE at the root of this distribution. +# +# SPDX-License-Identifier: Apache-2.0 OR MIT +# + +set(FILES + Tests/Clients/SimulationInterfacesTest.cpp +) diff --git a/Gems/SimulationInterfaces/Registry/assetprocessor_settings.setreg b/Gems/SimulationInterfaces/Registry/assetprocessor_settings.setreg new file mode 100644 index 0000000000..d7893eafaa --- /dev/null +++ b/Gems/SimulationInterfaces/Registry/assetprocessor_settings.setreg @@ -0,0 +1,18 @@ +{ + "Amazon": { + "AssetProcessor": { + "Settings": { + "ScanFolder SimulationInterfaces/Assets": { + "watch": "@GEMROOT:SimulationInterfaces@/Assets", + "recursive": 1, + "order": 101 + }, + "ScanFolder SimulationInterfaces/Registry": { + "watch": "@GEMROOT:SimulationInterfaces@/Registry", + "recursive": 1, + "order": 102 + } + } + } + } +} diff --git a/Gems/SimulationInterfaces/Registry/simulationinterface_settings.setreg b/Gems/SimulationInterfaces/Registry/simulationinterface_settings.setreg new file mode 100644 index 0000000000..3a5fb50cd3 --- /dev/null +++ b/Gems/SimulationInterfaces/Registry/simulationinterface_settings.setreg @@ -0,0 +1,6 @@ +{ + "SimulationInterfaces": { + "PrintStateNameInGui": true, + "StartInStoppedState": true + } +} diff --git a/Gems/SimulationInterfaces/gem.json b/Gems/SimulationInterfaces/gem.json new file mode 100644 index 0000000000..48ea321ff0 --- /dev/null +++ b/Gems/SimulationInterfaces/gem.json @@ -0,0 +1,33 @@ +{ + "gem_name": "SimulationInterfaces", + "version": "1.0.0", + "display_name": "SimulationInterfaces", + "license": "Apache-2.0 ", + "license_url": "https://opensource.org/licenses/Apache-2.0", + "origin": "RobotecAI", + "origin_url": "https://robotec.ai", + "type": "Code", + "summary": "This gem provides ROS 2 and C++ API for simulation interfaces.", + "canonical_tags": [ + "Gem" + ], + "user_tags": [ + "SimulationInterfaces", + "ROS2", + "ROS 2" + ], + "platforms": [ + "" + ], + "icon_path": "preview.png", + "requirements": "Requires ROS 2 Gem", + "documentation_url": "", + "dependencies": [ + "ROS2>=3.3.0", + "DebugDraw" + ], + "repo_uri": "", + "compatible_engines": [], + "engine_api_dependencies": [], + "restricted": "SimulationInterfaces" +} \ No newline at end of file diff --git a/Gems/SimulationInterfaces/preview.png b/Gems/SimulationInterfaces/preview.png new file mode 100644 index 0000000000..a33f8d2cf9 --- /dev/null +++ b/Gems/SimulationInterfaces/preview.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c2484ef49dcaa7474517f608e551ff2619ed5b1206febdd9c1d885b90b41c502 +size 4987