From 01f01987c9bb78a6010fb6cb2d271fbda7b7fd4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Fri, 21 Mar 2025 14:44:52 +0100 Subject: [PATCH 01/35] Added CMAKE logic to disable Contact Sensor and Spawner components. (#842) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Components will be disabled when gazebo_msgs is no longer available. Apply suggestions from code review --------- Signed-off-by: Michał Pełka Co-authored-by: Jan Hanca --- Gems/ROS2/Code/CMakeLists.txt | 20 +++++++++++++- .../ROS2ContactSensorComponent.cpp | 4 ++- .../ROS2ContactSensorComponent.h | 3 +++ Gems/ROS2/Code/Source/ROS2EditorModule.cpp | 7 +++-- Gems/ROS2/Code/Source/ROS2ModuleInterface.h | 17 +++++++----- .../Spawner/ROS2SpawnPointComponent.cpp | 4 ++- .../Source/Spawner/ROS2SpawnPointComponent.h | 4 ++- .../ROS2SpawnPointComponentController.cpp | 4 ++- .../ROS2SpawnPointComponentController.h | 4 ++- .../Spawner/ROS2SpawnPointEditorComponent.cpp | 4 ++- .../Spawner/ROS2SpawnPointEditorComponent.h | 4 ++- .../Source/Spawner/ROS2SpawnerComponent.cpp | 4 ++- .../Source/Spawner/ROS2SpawnerComponent.h | 4 ++- .../ROS2SpawnerComponentController.cpp | 4 ++- .../Spawner/ROS2SpawnerComponentController.h | 4 ++- .../Spawner/ROS2SpawnerEditorComponent.cpp | 4 ++- .../Spawner/ROS2SpawnerEditorComponent.h | 4 ++- Gems/ROS2/Code/ros2_editor_files.cmake | 15 ++++++++--- Gems/ROS2/Code/ros2_files.cmake | 26 ++++++++++++------- Gems/ROS2/Code/ros2_target_depends.cmake | 14 ++++++---- 20 files changed, 112 insertions(+), 42 deletions(-) diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index a7654f17a8..ac0e349d87 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -42,6 +42,18 @@ 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) +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 +83,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 @@ -161,6 +178,7 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS) Gem::Atom_Feature_Common.Public ) + # By default, we will specify that the above target ROS2 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 or both of the following lines: diff --git a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp index 8f0f8de88d..8c3ac813ac 100644 --- a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2ContactSensorComponent.h" #include #include diff --git a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h index 9584b4bae6..efba4ecb4e 100644 --- a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h +++ b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h @@ -7,6 +7,9 @@ */ #pragma once +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include #include 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..899893987a 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 @@ -80,8 +81,6 @@ namespace ROS2 RigidBodyTwistControlComponent::CreateDescriptor(), SkidSteeringControlComponent::CreateDescriptor(), ROS2CameraSensorComponent::CreateDescriptor(), - ROS2SpawnerComponent::CreateDescriptor(), - ROS2SpawnPointComponent::CreateDescriptor(), VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(), VehicleDynamics::WheelControllerComponent::CreateDescriptor(), VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(), @@ -96,9 +95,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/Spawner/ROS2SpawnPointComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp index a409add6b3..af2cfea763 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnPointComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h index da7ffa7b6a..ba2be3cab4 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h @@ -6,7 +6,9 @@ * */ #pragma once - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "Spawner/ROS2SpawnPointComponentController.h" #include #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp index d0aa41491a..06fc6a788c 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "Spawner/ROS2SpawnPointComponentController.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h index de241cc93a..9a510d3001 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h @@ -6,7 +6,9 @@ * */ #pragma once - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include #include #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp index b1d31087e5..7074bbb1fa 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2SpawnPointEditorComponent.h" #include "Spawner/ROS2SpawnPointComponentController.h" #include "Spawner/ROS2SpawnerEditorComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h index 56e79cb3d7..2c35fd7935 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h @@ -6,7 +6,9 @@ * */ #pragma once - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "Spawner/ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnPointComponentController.h" #include "Spawner/ROS2SpawnerEditorComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp index 9e9a6a9314..2498cc9a72 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2SpawnerComponent.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h index a9eb16aa65..5f3482ed29 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h @@ -6,7 +6,9 @@ * */ #pragma once - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp index fdec3e1998..210701514f 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2SpawnerComponentController.h" #include "Spawner/ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnerComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h index 19e07c596d..513cf7b6e8 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h @@ -7,7 +7,9 @@ */ #pragma once - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2/Spawner/SpawnerBus.h" #include "ROS2SpawnPointComponent.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp index dd667dda9f..df33f1a6ec 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp @@ -5,7 +5,9 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "ROS2SpawnerEditorComponent.h" #include "AzCore/Debug/Trace.h" #include "ROS2SpawnPointEditorComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h index 20cc92581e..72e099667b 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h @@ -6,7 +6,9 @@ * */ #pragma once - +#ifndef WITH_GAZEBO_MSGS +static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); +#endif #include "Spawner/ROS2SpawnerComponent.h" #include "Spawner/ROS2SpawnerComponentController.h" #include 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() From 9e440b2984b60cedf0c8dfc55f111f66ec243c85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 2 Apr 2025 12:43:03 +0200 Subject: [PATCH 02/35] Simulation interfaces gem - initial code and features (#848) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Spawning and spawnables Signed-off-by: Michał Pełka * Test and renaming Editor SystemComponents Signed-off-by: Michał Pełka * Added class skeleton Signed-off-by: Michał Pełka * prevent tool crashing Signed-off-by: Michał Pełka * physics stepping Signed-off-by: Michał Pełka * review comments Signed-off-by: Michał Pełka * Adjusted to review Signed-off-by: Michał Pełka --------- Signed-off-by: Michał Pełka --- Gems/SimulationInterfaces/.clang-format | 61 ++ Gems/SimulationInterfaces/.gitignore | 0 .../SampleAsset/TestSimulationEntity.prefab | 137 ++++ Gems/SimulationInterfaces/CMakeLists.txt | 11 + Gems/SimulationInterfaces/Code/CMakeLists.txt | 287 +++++++++ .../SimulationEntityManagerRequestBus.h | 102 +++ .../SimulationInterfacesTypeIds.h | 31 + .../SimulationMangerRequestBus.h | 45 ++ .../Code/Platform/Android/PAL_android.cmake | 9 + .../simulationinterfaces_api_files.cmake | 8 + .../simulationinterfaces_private_files.cmake | 13 + .../simulationinterfaces_shared_files.cmake | 13 + .../Code/Platform/Linux/PAL_linux.cmake | 9 + .../simulationinterfaces_api_files.cmake | 8 + ...imulationinterfaces_editor_api_files.cmake | 8 + .../simulationinterfaces_private_files.cmake | 13 + .../simulationinterfaces_shared_files.cmake | 13 + .../Code/Platform/Mac/PAL_mac.cmake | 9 + .../Mac/simulationinterfaces_api_files.cmake | 8 + ...imulationinterfaces_editor_api_files.cmake | 8 + .../simulationinterfaces_private_files.cmake | 13 + .../simulationinterfaces_shared_files.cmake | 13 + .../Code/Platform/Windows/PAL_windows.cmake | 9 + .../simulationinterfaces_api_files.cmake | 8 + ...imulationinterfaces_editor_api_files.cmake | 8 + .../simulationinterfaces_private_files.cmake | 13 + .../simulationinterfaces_shared_files.cmake | 13 + .../Code/Platform/iOS/PAL_ios.cmake | 9 + .../iOS/simulationinterfaces_api_files.cmake | 8 + .../simulationinterfaces_private_files.cmake | 13 + .../simulationinterfaces_shared_files.cmake | 13 + .../Code/Source/Clients/CommonUtilities.cpp | 31 + .../Code/Source/Clients/CommonUtilities.h | 20 + .../Code/Source/Clients/ConsoleCommands.inl | 224 +++++++ .../Clients/SimulationEntitiesManager.cpp | 586 ++++++++++++++++++ .../Clients/SimulationEntitiesManager.h | 94 +++ .../Clients/SimulationInterfacesModule.cpp | 27 + .../Code/Source/Clients/SimulationManager.cpp | 127 ++++ .../Code/Source/Clients/SimulationManager.h | 49 ++ .../SimulationInterfacesModuleInterface.cpp | 41 ++ .../SimulationInterfacesModuleInterface.h | 30 + .../Tools/SimulationEntitiesManagerEditor.cpp | 70 +++ .../Tools/SimulationEntitiesManagerEditor.h | 42 ++ .../SimulationInterfacesEditorModule.cpp | 48 ++ .../Source/Tools/SimulationManagerEditor.cpp | 68 ++ .../Source/Tools/SimulationManagerEditor.h | 43 ++ .../Clients/SimulationInterfacesTest.cpp | 11 + .../Tests/Tools/SimulationInterfaceTests.cpp | 170 +++++ .../Tests/Tools/SimulationIterfaceAppTest.cpp | 139 +++++ .../Code/Tests/Tools/TestFixture.cpp | 151 +++++ .../Code/Tests/Tools/TestFixture.h | 94 +++ .../Code/simulationinterfaces_api_files.cmake | 11 + ...imulationinterfaces_editor_api_files.cmake | 9 + ...simulationinterfaces_editor_app_test.cmake | 11 + ...ationinterfaces_editor_private_files.cmake | 12 + ...lationinterfaces_editor_shared_files.cmake | 9 + ...ulationinterfaces_editor_tests_files.cmake | 11 + .../simulationinterfaces_private_files.cmake | 16 + .../simulationinterfaces_shared_files.cmake | 9 + .../simulationinterfaces_tests_files.cmake | 9 + .../Registry/assetprocessor_settings.setreg | 18 + Gems/SimulationInterfaces/gem.json | 28 + Gems/SimulationInterfaces/preview.png | 3 + 63 files changed, 3124 insertions(+) create mode 100644 Gems/SimulationInterfaces/.clang-format create mode 100644 Gems/SimulationInterfaces/.gitignore create mode 100755 Gems/SimulationInterfaces/Assets/SampleAsset/TestSimulationEntity.prefab create mode 100644 Gems/SimulationInterfaces/CMakeLists.txt create mode 100644 Gems/SimulationInterfaces/Code/CMakeLists.txt create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h create mode 100644 Gems/SimulationInterfaces/Code/Platform/Android/PAL_android.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Android/simulationinterfaces_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_editor_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Linux/simulationinterfaces_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Mac/PAL_mac.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_editor_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Mac/simulationinterfaces_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Windows/PAL_windows.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_editor_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/Windows/simulationinterfaces_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/iOS/PAL_ios.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Platform/iOS/simulationinterfaces_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h create mode 100644 Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.h create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.h create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h create mode 100644 Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp create mode 100644 Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp create mode 100644 Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp create mode 100644 Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp create mode 100644 Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_editor_api_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_editor_app_test.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_editor_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_editor_tests_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_shared_files.cmake create mode 100644 Gems/SimulationInterfaces/Code/simulationinterfaces_tests_files.cmake create mode 100644 Gems/SimulationInterfaces/Registry/assetprocessor_settings.setreg create mode 100644 Gems/SimulationInterfaces/gem.json create mode 100644 Gems/SimulationInterfaces/preview.png 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/.gitignore b/Gems/SimulationInterfaces/.gitignore new file mode 100644 index 0000000000..e69de29bb2 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..47785f66fe --- /dev/null +++ b/Gems/SimulationInterfaces/Code/CMakeLists.txt @@ -0,0 +1,287 @@ +# 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 +) + +# 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_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 + ) + endif() + endif() +endif() diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h new file mode 100644 index 0000000000..f576420b7b --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.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 + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + //! # A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. + //! # The filters are combined with a logical AND. + //! @see EntityFilters.msg + struct EntityFilters + { + AZStd::string m_filter; //! A posix regular expression to match against entity names + AZStd::shared_ptr + m_bounds_shape; //! A shape to use for filtering entities, null means no bounds filtering + AZ::Transform m_bounds_pose{ AZ::Transform::CreateIdentity() }; + }; + + //! @see EntityState.msg + struct EntityState + { + AZ::Transform m_pose; //! The pose of the entity + AZ::Vector3 m_twist_linear; //! The linear velocity of the entity (in the entity frame) + AZ::Vector3 m_twist_angular; //! The angular velocity of the entity (in the entity frame) + }; + + struct Spawnable + { + AZStd::string m_uri; + AZStd::string m_description; + AZStd::string m_bounds_sphere; + }; + + 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 AZStd::vector GetEntities(const EntityFilters& filter) = 0; + + //! Get the state of an entity. + //! @see GetEntityState.srv + virtual EntityState GetEntityState(const AZStd::string& name) = 0; + + //! Get the state of all entities that match the filter. + //! @see GetEntitiesStates.srv + virtual AZStd::unordered_map GetEntitiesStates(const EntityFilters& filter) = 0; + + //! Set the state of an entity. + //! @see SetEntityState.srv + virtual bool SetEntityState(const AZStd::string& name, const EntityState& state) = 0; + + //! Remove previously spawned entity from the simulation. + //! @see DeleteEntity.srv + virtual bool DeleteEntity(const AZStd::string& name) = 0; + + virtual AZStd::vector 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 is empty, if the entity could not be registered (e.g. prefab has no simulated entities) + using SpawnCompletedCb = AZStd::function&)>; + + virtual void SpawnEntity( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + SpawnCompletedCb completedCb) = 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/SimulationInterfacesTypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h new file mode 100644 index 0000000000..7d165b3ed6 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.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 + +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}"; + + // 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}"; + +} // 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..2b622c912c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.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 + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + + class SimulationManagerRequests + { + public: + AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); + virtual ~SimulationManagerRequests() = default; + + virtual void SetSimulationPaused(bool paused) = 0; + virtual void StepSimulation(AZ::u32 steps) = 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; + +} // 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..c79341aef2 --- /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 TRUE) +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/Clients/CommonUtilities.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp new file mode 100644 index 0000000000..d50ffb57e1 --- /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 \ No newline at end of file diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h new file mode 100644 index 0000000000..0d0a96a802 --- /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 \ No newline at end of file diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl new file mode 100644 index 0000000000..59e010960d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl @@ -0,0 +1,224 @@ +/* + * 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 + +namespace SimulationInterfacesCommands +{ + + using namespace SimulationInterfaces; + static void simulationinterfaces_GetEntities(const AZ::ConsoleCommandContainer& arguments) + { + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + AZ_Printf("SimulationInterfacesConsole", "Number of simulation entities: %d\n", entities.size()); + for (const auto& entity : entities) + { + AZ_Printf("SimulationInterfacesConsole", " - %s\n", entity.c_str()); + } + } + + static void simulationinterfaces_Pause(const AZ::ConsoleCommandContainer& arguments) + { + SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::SetSimulationPaused, true); + } + + static void simulationinterfaces_Resume(const AZ::ConsoleCommandContainer& arguments) + { + SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::SetSimulationPaused, false); + } + + + static void simulationinterfaces_Step(const AZ::ConsoleCommandContainer& arguments) + { + if (arguments.empty()) + { + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_Step \n"); + return; + } + uint32_t steps = AZStd::stoi(AZStd::string(arguments[0])); + + SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::StepSimulation, steps); + } + + + + static void simulationinterfaces_GetEntitiesSphere(const AZ::ConsoleCommandContainer& arguments) + { + float sphereShape = 10.f; + AZ::Vector3 position = AZ::Vector3::CreateZero(); + sphereShape = arguments.empty() ? 10.f : (AZStd::stof(AZStd::string(arguments[0]))); + position.SetX(arguments.size() > 1 ? (AZStd::stof(AZStd::string(arguments[1]))) : 0.f); + position.SetY(arguments.size() > 2 ? (AZStd::stof(AZStd::string(arguments[2]))) : 0.f); + position.SetZ(arguments.size() > 3 ? (AZStd::stof(AZStd::string(arguments[3]))) : 0.f); + + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntities in radius %f \n", sphereShape); + AZ_Printf("SimulationInterfacesConsole", "position %f %f %f \n", position.GetX(), position.GetY(), position.GetZ()); + EntityFilters filter; + filter.m_bounds_shape = AZStd::make_shared(sphereShape); + + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + AZ_Printf("SimulationInterfacesConsole", "Number of simulation entities: %d\n", entities.size()); + for (const auto& entity : entities) + { + AZ_Printf("SimulationInterfacesConsole", " - %s\n", entity.c_str()); + } + } + + static void simulationinterfaces_GetEntityState(const AZ::ConsoleCommandContainer& arguments) + { + if (arguments.empty()) + { + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState requires entity name\n"); + return; + } + const AZStd::string entityName = arguments[0]; + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState %s\n", entityName.c_str()); + EntityState entityState; + SimulationEntityManagerRequestBus::BroadcastResult(entityState, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + AZ_Printf("SimulationInterfacesConsole", "Entity %s\n", entityName.c_str()); + AZ_Printf( + "SimulationInterfacesConsole", + "Pose %s\n", + AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str()); + AZ_Printf( + "SimulationInterfacesConsole", + "Rotation %s \n", + AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str()); + + const AZ::Vector3 euler = entityState.m_pose.GetRotation().GetEulerDegrees(); + AZ_Printf("SimulationInterfacesConsole", "Rotation (euler) %s\n", AZ::Vector3ToString(euler).c_str()); + AZ_Printf( + "SimulationInterfacesConsole", + "Twist Linear %s\n", + AZ::Vector3ToString(entityState.m_twist_linear).c_str()); + AZ_Printf( + "SimulationInterfacesConsole", + "Twist Angular %s\n", + AZ::Vector3ToString(entityState.m_twist_angular).c_str()); + } + + static void simulationinterfaces_SetStateXYZ(const AZ::ConsoleCommandContainer& arguments) + { + if (arguments.empty()) + { + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState requires entity name\n"); + return; + } + const AZStd::string entityName = arguments[0]; + AZ::Vector3 position = AZ::Vector3::CreateZero(); + position.SetX(arguments.size() > 1 ? (AZStd::stof(AZStd::string(arguments[1]))) : 0.f); + position.SetY(arguments.size() > 2 ? (AZStd::stof(AZStd::string(arguments[2]))) : 0.f); + position.SetZ(arguments.size() > 3 ? (AZStd::stof(AZStd::string(arguments[3]))) : 0.f); + EntityState entityState{}; + entityState.m_pose = AZ::Transform::CreateIdentity(); + entityState.m_pose.SetTranslation(position); + bool isOk = false; + SimulationEntityManagerRequestBus::BroadcastResult( + isOk, &SimulationEntityManagerRequestBus::Events::SetEntityState, entityName, entityState); + if (isOk) + { + AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str()); + } + else + { + AZ_Printf("SimulationInterfacesConsole", "Entity %s state NOT set\n", entityName.c_str()); + } + } + + static void simulationinterfaces_DeleteEntity(const AZ::ConsoleCommandContainer& arguments) + { + if (arguments.empty()) + { + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_DeleteEntity requires entity name\n"); + return; + } + const AZStd::string entityName = arguments[0]; + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_DeleteEntity %s\n", entityName.c_str()); + bool isOk = false; + SimulationEntityManagerRequestBus::BroadcastResult(isOk, &SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName); + if (isOk) + { + AZ_Printf("SimulationInterfacesConsole", "Entity %s deleted\n", entityName.c_str()); + } + else + { + AZ_Printf("SimulationInterfacesConsole", "Entity %s NOT deleted\n", entityName.c_str()); + } + } + + static void simulationinterfaces_GetSpawnables(const AZ::ConsoleCommandContainer& arguments) + { + AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetSpawnables\n"); + AZStd::vector spawnables; + SimulationEntityManagerRequestBus::BroadcastResult(spawnables, &SimulationEntityManagerRequestBus::Events::GetSpawnables); + AZ_Printf("SimulationInterfacesConsole", "Number of spawnables: %d\n", spawnables.size()); + for (const auto& spawnable : spawnables) + { + AZ_Printf("SimulationInterfaces", " - %s\n", spawnable.m_uri.c_str()); + } + } + + static void simulationinterfaces_Spawn(const AZ::ConsoleCommandContainer& arguments) + { + if (arguments.size() < 2) + { + AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn minimal :\n"); + AZ_Printf("SimulationInterfacesConsole", " simulationinterface_Spawn \n"); + AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn optional :\n"); + AZ_Printf("SimulationInterfacesConsole", " simulationinterface_Spawn \n"); + return; + } + AZStd::string name = arguments[0]; + AZStd::string uri = arguments[1]; + AZStd::string entityNamespace = arguments.size() > 2 ? arguments[2] : ""; + AZ::Transform initialPose = AZ::Transform::CreateIdentity(); + if (arguments.size() > 5) + { + initialPose.SetTranslation( + AZ::Vector3( + AZStd::stof(AZStd::string(arguments[3])), + AZStd::stof(AZStd::string(arguments[4])), + AZStd::stof(AZStd::string(arguments[5])))); + } + SimulationEntityManagerRequests::SpawnCompletedCb completedCb = [](const AZ::Outcome& name) + { + if (name.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "Entity %s spawned and registered\n", name.GetValue().c_str()); + } + else + { + AZ_Printf("SimulationInterfacesConsole", "Entity NOT spawned. Error : %s\n", name.GetError().c_str()); + } + }; + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, completedCb); + AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); + } + + AZ_CONSOLEFREEFUNC(simulationinterfaces_Pause, AZ::ConsoleFunctorFlags::DontReplicate, "Pause simulation."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_Resume, AZ::ConsoleFunctorFlags::DontReplicate, "Resume simulation."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_Step, AZ::ConsoleFunctorFlags::DontReplicate, "Step simulation."); + + AZ_CONSOLEFREEFUNC( + simulationinterfaces_GetEntities, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the scene."); + AZ_CONSOLEFREEFUNC( + simulationinterfaces_GetEntitiesSphere, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the radius."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_GetEntityState, AZ::ConsoleFunctorFlags::DontReplicate, "Get state of the entity."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_SetStateXYZ, AZ::ConsoleFunctorFlags::DontReplicate, "Set state of the entity."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_DeleteEntity, AZ::ConsoleFunctorFlags::DontReplicate, "Delete entity."); + AZ_CONSOLEFREEFUNC( + simulationinterfaces_GetSpawnables, AZ::ConsoleFunctorFlags::DontReplicate, "Get all spawnable entities in the scene."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_Spawn, AZ::ConsoleFunctorFlags::DontReplicate, "Spawn entity."); +} // namespace SimulationInterfacesCommands diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp new file mode 100644 index 0000000000..54ac45bd2e --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -0,0 +1,586 @@ +/* + * 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 "CommonUtilities.h" +#include "ConsoleCommands.inl" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) + { + if (!state.m_twist_angular.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + { + // get transform + AZ::Vector3 angularVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_angular); + rigidBody->SetAngularVelocity(angularVelWorld); + } + + if (!state.m_twist_linear.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + { + // get transform + AZ::Vector3 linearVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_linear); + rigidBody->SetAngularVelocity(linearVelWorld); + } + } + + 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")); + } + + void SimulationEntitiesManager::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + dependent.push_back(AZ_CRC_CE("PhysicsService")); + } + + SimulationEntitiesManager::SimulationEntitiesManager() + { + if (SimulationEntityManagerInterface::Get() == nullptr) + { + SimulationEntityManagerInterface::Register(this); + } + } + + SimulationEntitiesManager::~SimulationEntitiesManager() + { + if (SimulationEntityManagerInterface::Get() == this) + { + SimulationEntityManagerInterface::Unregister(this); + } + } + + void SimulationEntitiesManager::Init() + { + } + + 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; + } + + void SimulationEntitiesManager::Activate() + { + m_simulationBodyAddedHandler = AzPhysics::SceneEvents::OnSimulationBodyAdded::Handler( + [this](AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) + { + auto* scene = GetSceneHelper(sceneHandle); + if (scene == nullptr) + { + return; + } + auto* body = scene->GetSimulatedBodyFromHandle(bodyHandle); + AZ_Assert(body, "Simulated body is not available."); + 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(); + AZ::Entity* entity = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId); + AZ_Assert(entity, "Entity is not available."); + // check if entity is not spawned by this component + const auto ticketId = entity->GetEntitySpawnTicketId(); + AZStd::string proposedName{}; + // check if ticket is in the unregistered list + + auto spawnData = m_spawnCompletedCallbacks.find(ticketId); + if (spawnData != m_spawnCompletedCallbacks.end()) + { + proposedName = spawnData->second.m_userProposedName; + } + + const AZStd::string registeredName = this->AddSimulatedEntity(entityId, proposedName); + // call the callback + if (spawnData != m_spawnCompletedCallbacks.end()) + { + // call and remove the callback + spawnData->second.m_completedCb(AZ::Success(registeredName)); + m_spawnCompletedCallbacks.erase(spawnData); + } + }); + 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, "Hmm, we already have a 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(); + } + } + + void SimulationEntitiesManager::Deactivate() + { + SimulationEntityManagerRequestBus::Handler::BusDisconnect(); + if (m_simulationBodyAddedHandler.IsConnected()) + { + m_simulationBodyAddedHandler.Disconnect(); + } + if (m_simulationBodyRemovedHandler.IsConnected()) + { + m_simulationBodyRemovedHandler.Disconnect(); + } + m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; + if (m_sceneAddedHandler.IsConnected()) + { + m_sceneAddedHandler.Disconnect(); + } + if (m_sceneAddedHandler.IsConnected()) + { + 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; + AZ_Printf("SimulationInterfaces", "Registered entity %s\n", simulatedEntityName.c_str()); + return simulatedEntityName; + } + + void SimulationEntitiesManager::RemoveSimulatedEntity(AZ::EntityId entityId) + { + auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); + if (findIt != m_entityIdToSimulatedEntityMap.end()) + { + const auto& simulatedEntityName = findIt->second; + m_entityIdToSimulatedEntityMap.erase(findIt); + m_simulatedEntityToEntityIdMap.erase(simulatedEntityName); + } + } + + AZStd::vector SimulationEntitiesManager::GetEntities(const EntityFilters& filter) + { + const bool reFilter = !filter.m_filter.empty(); + const bool shapeCastFilter = filter.m_bounds_shape != 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 entities; + } + + AzPhysics::OverlapRequest request; + request.m_shapeConfiguration = filter.m_bounds_shape; + request.m_pose = filter.m_bounds_pose; + 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; + auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); + if (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_filter); + if (regex.Valid()) + { + AZStd::ranges::copy_if( + prefilteredEntities, + AZStd::back_inserter(entities), + [®ex](const AZStd::string& entityName) + { + return AZStd::regex_search(entityName, regex); + }); + } + } + return entities; + } + + EntityState SimulationEntitiesManager::GetEntityState(const AZStd::string& name) + { + const auto findIt = m_simulatedEntityToEntityIdMap.find(name); + AZ_Error("SimulationInterfaces", findIt != m_simulatedEntityToEntityIdMap.end(), "Entity %s not found", name.c_str()); + if (findIt != m_simulatedEntityToEntityIdMap.end()) + { + 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 + AZ::Transform entityTransformInv = entityState.m_pose.GetInverse(); + entityState.m_twist_linear = entityTransformInv.TransformVector(linearVelocity); + entityState.m_twist_angular = entityTransformInv.TransformVector(angularVelocity); + return entityState; + } + return {}; + } + + bool SimulationEntitiesManager::SetEntityState(const AZStd::string& name, const EntityState& state) + { + const auto findIt = m_simulatedEntityToEntityIdMap.find(name); + if (findIt != m_simulatedEntityToEntityIdMap.end()) + { + 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); + + if (state.m_pose.IsOrthogonal()) + { + // disable simulation for all entities + AZStd::map entityTransforms; + for (const auto& descendant : entityAndDescendants) + { + // get name + AZStd::string entityName = "Unknown"; + AZ::ComponentApplicationBus::BroadcastResult(entityName, &AZ::ComponentApplicationRequests::GetEntityName, descendant); + AZ_Printf("SimulationInterfaces", "Disable physics for entity %s\n", entityName.c_str()); + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics); + } + + AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose); + + for (const auto& descendant : entityAndDescendants) + { + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::EnablePhysics); + Physics::RigidBodyRequestBus::Event( + descendant, &Physics::RigidBodyRequests::SetAngularVelocity, AZ::Vector3::CreateZero()); + Physics::RigidBodyRequestBus::Event( + descendant, &Physics::RigidBodyRequests::SetLinearVelocity, AZ::Vector3::CreateZero()); + } + } + if (!state.m_twist_linear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twist_angular.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); + } + } + } + return false; + } + + bool SimulationEntitiesManager::DeleteEntity(const AZStd::string& name) + { + const auto findIt = m_simulatedEntityToEntityIdMap.find(name); + + if (findIt == m_simulatedEntityToEntityIdMap.end()) + { + return false; + } + + 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."); + // check if entity is spawned by this component + const auto ticketId = entity->GetEntitySpawnTicketId(); + if (m_spawnedTickets.find(ticketId) != m_spawnedTickets.end()) + { + // remove the ticket + m_spawnedTickets.erase(ticketId); + } + else + { + AZ_Warning("SimulationInterfaces", false, "Entity %s was not spawned by this component, wont delete it", name.c_str()); + return false; + } +#ifdef POTENTIALY_UNSAFE + if (findIt != m_simulatedEntityToEntityIdMap.end()) + { + const AZ::EntityId entityId = findIt->second; + AZ_Assert(entityId.IsValid(), "EntityId is not valid"); + // get all descendants + AZStd::vector entityAndDescendants; + AZ::TransformBus::EventResult(entityAndDescendants, entityId, &AZ::TransformBus::Events::GetEntityAndAllDescendants); + for (const auto& descendant : entityAndDescendants) + { + // I am not sure if this is the safe way to delete an entity + AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationRequests::DeleteEntity, descendant); + } + + return true; + } +#endif + return false; + } + + AZStd::unordered_map SimulationEntitiesManager::GetEntitiesStates(const EntityFilters& filter) + { + AZStd::unordered_map entitiesStates; + const auto& entities = GetEntities(filter); + for (const auto& entity : entities) + { + entitiesStates.emplace(AZStd::make_pair(entity, GetEntityState(entity))); + } + return entitiesStates; + } + + AZStd::vector 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 spawnables; + } + + void SimulationEntitiesManager::SpawnEntity( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + SpawnCompletedCb completedCb) + { + // get rel path from uri + const AZStd::string relPath = Utils::UriToRelPath(uri); + + // create spawnnable + 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) + { + completedCb(AZ::Failure("Failed to get spawnable asset - incorrect uri")); + return; + } + + auto ticket = AzFramework::EntitySpawnTicket(spawnableAsset); + AzFramework::SpawnAllEntitiesOptionalArgs optionalArgs; + + optionalArgs.m_preInsertionCallback = [initialPose, entityNamespace, name](auto id, auto view) + { + if (view.empty()) + { + return; + } + const AZ::Entity* root = *view.begin(); + + // change names for all entites + for (auto* entity : view) + { + AZStd::string entityName = AZStd::string::format("%s_%s", name.c_str(), entity->GetName().c_str()); + entity->SetName(entityName); + } + + auto* transformInterface = root->FindComponent(); + if (transformInterface) + { + transformInterface->SetWorldTM(initialPose); + } + + if (!entityNamespace.empty()) + { + // TODO: Mpelka set ROS 2 namespace here + AZ_Error("SimulationInterfaces", false, "ROS 2 namespace is not implemented yet in spawning"); + } + }; + optionalArgs.m_completionCallback = + [this](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) + { + // 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 AZFrameworrk::Pshysics::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 and remove the callback + spawnData->second.m_completedCb(AZ::Failure( + "Entity was not registered in simulation interface - no physics component or physics component is not enabled.")); + m_spawnCompletedCallbacks.erase(spawnData); + } + }; + + spawner->SpawnAllEntities(ticket, optionalArgs); + + auto ticketId = ticket.GetId(); + AZ_Printf("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 entityName = proposedName; + if (entityName.empty()) + { + AZ::ComponentApplicationBus::BroadcastResult(entityName, &AZ::ComponentApplicationRequests::GetEntityName, entityId); + } + // Generate unique simulated entity name + AZStd::string simulatedEntityName = entityName; + // check if name is unique + auto otherEntityIt = m_simulatedEntityToEntityIdMap.find(simulatedEntityName); + if (otherEntityIt != m_simulatedEntityToEntityIdMap.end()) + { + // name is not unique, add entityId to name + simulatedEntityName = AZStd::string::format("%s_%s", entityName.c_str(), entityId.ToString().c_str()); + } + return simulatedEntityName; + } +} // 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..25412f21b3 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -0,0 +1,94 @@ +/* + * 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 + { + 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(); + + protected: + // SimulationEntityManagerRequestBus interface implementation + AZStd::vector GetEntities(const EntityFilters& filter) override; + EntityState GetEntityState(const AZStd::string& name) override; + AZStd::unordered_map GetEntitiesStates(const EntityFilters& filter) override; + bool SetEntityState(const AZStd::string& name, const EntityState& state) override; + bool DeleteEntity(const AZStd::string& name) override; + AZStd::vector GetSpawnables() override; + void SpawnEntity( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + SpawnCompletedCb completedCb) override; + + // AZ::Component interface implementation + void Init() override; + void Activate() override; + void Deactivate() override; + + private: + //! 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; + + 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::unordered_map m_simulatedEntityToEntityIdMap; + AZStd::unordered_map m_entityIdToSimulatedEntityMap; + AZStd::unordered_set m_disabledBodies; + + AZStd::unordered_map m_spawnedTickets; + + struct SpawnCompletedCbData + { + AZStd::string m_userProposedName; //! Name proposed by the User in spawn request + SpawnCompletedCb m_completedCb; //! User callback to be called when the entity is registered + AZ::ScriptTimePoint m_spawnCompletedTime; //! Time at which the entity was spawned + }; + AZStd::unordered_map + m_spawnCompletedCallbacks; //! Callbacks to be called when the entity is registered + }; + +} // 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..119dec747c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp @@ -0,0 +1,27 @@ +/* + * 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 + +#if defined(O3DE_GEM_NAME) +AZ_DECLARE_MODULE_CLASS(AZ_JOIN(Gem_, O3DE_GEM_NAME), SimulationInterfaces::SimulationInterfacesModule) +#else +AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfaces, SimulationInterfaces::SimulationInterfacesModule) +#endif diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp new file mode 100644 index 0000000000..67233c9a45 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -0,0 +1,127 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "SimulationManager.h" + +#include +#include +#include +#include + +namespace SimulationInterfaces +{ + + 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")); + } + + void SimulationManager::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + } + + SimulationManager::SimulationManager() + { + if (SimulationManagerRequestBusInterface::Get() == nullptr) + { + SimulationManagerRequestBusInterface::Register(this); + } + } + + SimulationManager::~SimulationManager() + { + if (SimulationManagerRequestBusInterface::Get() == this) + { + SimulationManagerRequestBusInterface::Unregister(this); + } + } + + void SimulationManager::Init() + { + } + + void SimulationManager::Activate() + { + SimulationManagerRequestBus::Handler::BusConnect(); + } + + void SimulationManager::Deactivate() + { + SimulationManagerRequestBus::Handler::BusDisconnect(); + } + + 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(); + 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); + } + } + + void SimulationManager::StepSimulation(AZ::u32 steps) + { + 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); + if (m_numberOfPhysicsSteps <= 0) + { + SetSimulationPaused(true); + // remove handler + m_simulationFinishEvent.Disconnect(); + } + }); + + // get default scene + 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); + + } + +} // 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..8212e8e128 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -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 + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +namespace SimulationInterfaces +{ + class SimulationManager : + public AZ::Component, + protected SimulationManagerRequestBus::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 Init() override; + void Activate() override; + void Deactivate() override; + protected: + void SetSimulationPaused(bool paused) override; + void StepSimulation(AZ::u32 steps) override; + uint32_t m_numberOfPhysicsSteps = 0; + AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; + + }; +} // namespace SimulationInterfaces \ No newline at end of file diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp new file mode 100644 index 0000000000..ec2ff7c74f --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -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 + * + */ + +#include "SimulationInterfacesModuleInterface.h" +#include + +#include + +#include +#include "Clients/SimulationManager.h" + +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(), + }); + } + + AZ::ComponentTypeList SimulationInterfacesModuleInterface::GetRequiredSystemComponents() const + { + return AZ::ComponentTypeList{ + 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/SimulationEntitiesManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp new file mode 100644 index 0000000000..c20439b84b --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp @@ -0,0 +1,70 @@ +/* + * 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); + } + + void SimulationEntitiesManagerEditor::GetDependentServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + } + + 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/SimulationInterfacesEditorModule.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp new file mode 100644 index 0000000000..62eb71feb4 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp @@ -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 + * + */ + +#include "SimulationEntitiesManagerEditor.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(), + }); + } + + /** + * Add required SystemComponents to the SystemEntity. + * Non-SystemComponents should not be added here + */ + AZ::ComponentTypeList GetRequiredSystemComponents() const override + { + return AZ::ComponentTypeList{ + azrtti_typeid(), + }; + } + }; +} // namespace SimulationInterfaces + +#if defined(O3DE_GEM_NAME) +AZ_DECLARE_MODULE_CLASS(AZ_JOIN(Gem_, O3DE_GEM_NAME, _Editor), SimulationInterfaces::SimulationInterfacesEditorModule) +#else +AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfaces_Editor, SimulationInterfaces::SimulationInterfacesEditorModule) +#endif diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp new file mode 100644 index 0000000000..d0366d858d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp @@ -0,0 +1,68 @@ +/* + * 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); + } + + void SimulationManagerEditor::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + } + void SimulationManagerEditor::Init() + { + BaseSystemComponent::Init(); + } + + void SimulationManagerEditor::Activate() + { + BaseSystemComponent::Activate(); + AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + } + + void SimulationManagerEditor::Deactivate() + { + AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); + BaseSystemComponent::Deactivate(); + } + +} // 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..cfec99885a --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h @@ -0,0 +1,43 @@ +/* +* 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 SimulationManagerEditor + : public SimulationManager + , protected AzToolsFramework::EditorEvents::Bus::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; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp new file mode 100644 index 0000000000..40217ff9bc --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp @@ -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 + * + */ + +#include + +AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp new file mode 100644 index 0000000000..17cb23c95c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp @@ -0,0 +1,170 @@ + +/* + * 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; + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + EXPECT_EQ(entities.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()); + + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_EQ(entities.size(), 2); + DeleteEntity(entityId1); + + AZStd::vector entities2; + SimulationEntityManagerRequestBus::BroadcastResult(entities2, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + EXPECT_EQ(entities2.size(), 1); + + DeleteEntity(entityId2); + AZStd::vector entities3; + SimulationEntityManagerRequestBus::BroadcastResult(entities3, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + EXPECT_EQ(entities3.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()); + AZStd::vector entities; + + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + EXPECT_EQ(entities.size(), 2); + EXPECT_NE(entities[0], entities[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_bounds_shape = AZStd::make_shared(2.0f); + + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + auto* physicsSystem = AZ::Interface::Get(); + physicsSystem->Simulate(1.0f / 60.0f); + + ASSERT_EQ(entities.size(), 1); + EXPECT_EQ(entities.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_filter = "Will.*"; + + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + + ASSERT_EQ(entities.size(), 1); + EXPECT_EQ(entities.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_filter = "[a-z"; + + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + + EXPECT_EQ(entities.size(), 0); + 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; + EntityState stateBefore; + SimulationEntityManagerRequestBus::BroadcastResult(stateBefore, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + 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); + } + EntityState stateAfter; + SimulationEntityManagerRequestBus::BroadcastResult(stateAfter, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + 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_twist_linear.GetLength(), 0.0f); + + DeleteEntity(entityId1); + } + +} // 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..d389b3ba6f --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -0,0 +1,139 @@ + +/* + * 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()); + } + + + 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; + SimulationEntityManagerRequests::SpawnCompletedCb completedCb = [&](const AZ::Outcome& result) + { + EXPECT_TRUE(result.IsSuccess()); + completed = true; + }; + + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, completedCb); + + // entities are spawned asynchronously, so we need to tick the app to let the entity be spawned + TickApp(100); + EXPECT_TRUE(completed); + + // list simulation entities + AZStd::vector entities; + SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + 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, + AZStd::unordered_map entityStates; + SimulationEntityManagerRequestBus::BroadcastResult( + entityStates, &SimulationEntityManagerRequestBus::Events::GetEntitiesStates, EntityFilters()); + 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_bounds_shape = AZStd::make_shared(2.0f); + filter.m_bounds_pose = AZ::Transform::CreateTranslation(AZ::Vector3(1000.0f, 0.0f, 0.0f)); + AZStd::vector entitiesFiltered; + SimulationEntityManagerRequestBus::BroadcastResult(entitiesFiltered, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); + EXPECT_EQ(entitiesFiltered.size(), 1); + + // delete entity using its name + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName); + TickApp(100); + + // list simulation entities after deletion, expect no simulation entities + AZStd::vector entities2; + SimulationEntityManagerRequestBus::BroadcastResult(entities2, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + EXPECT_EQ(entities2.size(), 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..9eec9d8e55 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp @@ -0,0 +1,151 @@ + +/* + * 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..5e2f1d4e8d --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h @@ -0,0 +1,94 @@ + +/* + * 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 + +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/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake new file mode 100644 index 0000000000..329f4a5b87 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_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 + Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h + Include/SimulationInterfaces/SimulationMangerRequestBus.h + Include/SimulationInterfaces/SimulationInterfacesTypeIds.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..2b0af4483c --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_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 + Source/Tools/SimulationManagerEditor.cpp + Source/Tools/SimulationManagerEditor.h + Source/Tools/SimulationEntitiesManagerEditor.cpp + Source/Tools/SimulationEntitiesManagerEditor.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..8947b47d86 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_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/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 +) 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/gem.json b/Gems/SimulationInterfaces/gem.json new file mode 100644 index 0000000000..6f1244c140 --- /dev/null +++ b/Gems/SimulationInterfaces/gem.json @@ -0,0 +1,28 @@ +{ + "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 C++ API for simulation interfaces.", + "canonical_tags": [ + "Gem" + ], + "user_tags": [ + "SimulationInterfaces", "ROS2", "ROS 2" + ], + "platforms": [ + "" + ], + "icon_path": "preview.png", + "requirements": "", + "documentation_url": "", + "dependencies": [], + "repo_uri": "", + "compatible_engines": [], + "engine_api_dependencies": [], + "restricted": "SimulationInterfaces" +} 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 From bf040761d04ae34c2bf07a5c231e12670a859b93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 3 Apr 2025 14:55:49 +0100 Subject: [PATCH 03/35] [SimulationIterfaces] Refactor API to use heavily AZ::Outcome and error codes from standard (#850) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Refactor API to use heavily AZ::Outcome and error codes from standard * asynhronous despawning * Adjust to review -- Signed-off-by: Michał Pełka --- .../Include/SimulationInterfaces/Result.h | 36 +++ .../SimulationEntityManagerRequestBus.h | 27 ++- .../SimulationMangerRequestBus.h | 6 +- .../Include/SimulationInterfaces/TagFilter.h | 30 +++ .../Code/Source/Clients/ConsoleCommands.inl | 102 ++++---- .../Clients/SimulationEntitiesManager.cpp | 222 +++++++++++------- .../Clients/SimulationEntitiesManager.h | 12 +- .../Code/Source/Clients/SimulationManager.cpp | 1 - .../Code/Source/Clients/SimulationManager.h | 10 +- .../SimulationInterfacesModuleInterface.cpp | 2 +- .../Tools/SimulationEntitiesManagerEditor.cpp | 11 +- .../Source/Tools/SimulationManagerEditor.h | 64 ++--- .../Tests/Tools/SimulationInterfaceTests.cpp | 78 +++--- .../Tests/Tools/SimulationIterfaceAppTest.cpp | 49 ++-- .../Code/Tests/Tools/TestFixture.cpp | 1 - .../Code/simulationinterfaces_api_files.cmake | 6 +- 16 files changed, 417 insertions(+), 240 deletions(-) create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h new file mode 100644 index 0000000000..3d449a1959 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.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 +#include +namespace SimulationInterfaces +{ + //! Result codes to be used in the Result message + //! @see Result.msg + enum class ErrorCode + { + RESULT_FEATURE_UNSUPPORTED = 0, + RESULT_NOT_FOUND = 2, + RESULT_INCORRECT_STATE = 3, + RESULT_OPERATION_FAILED = 4 + }; + + //! A message type to represent the result of a failed operation + struct FailedResult + { + FailedResult() = default; + FailedResult(ErrorCode error_code, const AZStd::string& error_string) + : error_code(error_code) + , error_string(error_string) + { + } + ErrorCode error_code; + AZStd::string error_string; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index f576420b7b..d07941b206 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -10,6 +10,8 @@ #include +#include "Result.h" +#include "TagFilter.h" #include #include #include @@ -18,11 +20,11 @@ namespace SimulationInterfaces { //! # A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. - //! # The filters are combined with a logical AND. //! @see EntityFilters.msg struct EntityFilters { AZStd::string m_filter; //! A posix regular expression to match against entity names + TagFilter m_tags_filter; //! A filter to match against entity tags AZStd::shared_ptr m_bounds_shape; //! A shape to use for filtering entities, null means no bounds filtering AZ::Transform m_bounds_pose{ AZ::Transform::CreateIdentity() }; @@ -43,6 +45,11 @@ namespace SimulationInterfaces AZStd::string m_bounds_sphere; }; + using EntityNameList = AZStd::vector; + using MultipleEntitiesStates = AZStd::unordered_map; + using SpawnableList = AZStd::vector; + using DeletionCompletedCb = AZStd::function&)>; + using SpawnCompletedCb = AZStd::function&)>; class SimulationEntityManagerRequests { public: @@ -54,29 +61,31 @@ namespace SimulationInterfaces //! - 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 AZStd::vector GetEntities(const EntityFilters& filter) = 0; + virtual AZ::Outcome GetEntities(const EntityFilters& filter) = 0; //! Get the state of an entity. //! @see GetEntityState.srv - virtual EntityState GetEntityState(const AZStd::string& name) = 0; + virtual AZ::Outcome GetEntityState(const AZStd::string& name) = 0; //! Get the state of all entities that match the filter. - //! @see GetEntitiesStates.srv - virtual AZStd::unordered_map GetEntitiesStates(const EntityFilters& filter) = 0; + //! @see GetEntitiesStates.srv + virtual AZ::Outcome GetEntitiesStates(const EntityFilters& filter) = 0; //! Set the state of an entity. //! @see SetEntityState.srv - virtual bool SetEntityState(const AZStd::string& name, const EntityState& state) = 0; + virtual AZ::Outcome SetEntityState(const AZStd::string& name, const EntityState& state) = 0; //! Remove previously spawned entity from the simulation. //! @see DeleteEntity.srv - virtual bool DeleteEntity(const AZStd::string& name) = 0; + virtual void DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) = 0; - virtual AZStd::vector GetSpawnables() = 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 is empty, if the entity could not be registered (e.g. prefab has no simulated entities) - using SpawnCompletedCb = AZStd::function&)>; virtual void SpawnEntity( const AZStd::string& name, diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 2b622c912c..05bc8cbc34 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -24,9 +24,13 @@ namespace SimulationInterfaces AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); virtual ~SimulationManagerRequests() = default; + //! Set the simulation to paused or unpaused, + //! expect always to succeed virtual void SetSimulationPaused(bool paused) = 0; - virtual void StepSimulation(AZ::u32 steps) = 0; + //! Step the simulation by a number of steps + //! expect always to succeed + virtual void StepSimulation(AZ::u32 steps) = 0; }; class SimulationMangerRequestBusTraits : public AZ::EBusTraits diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h new file mode 100644 index 0000000000..cdd0660a05 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.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 + * + */ + +#pragma once + +#include +#include + +namespace SimulationInterfaces +{ + //! Structure to design a filter for tags + //! @see TagsFilter.msg + + struct TagFilter + { + enum class TagFilterMode + { + FILTER_MODE_ANY, + FILTER_MODE_ALL + }; + AZStd::unordered_set m_tags; + TagFilterMode m_mode; + }; + +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl index 59e010960d..ef5ecbaf85 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl @@ -19,10 +19,15 @@ namespace SimulationInterfacesCommands using namespace SimulationInterfaces; static void simulationinterfaces_GetEntities(const AZ::ConsoleCommandContainer& arguments) { - AZStd::vector entities; + AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - AZ_Printf("SimulationInterfacesConsole", "Number of simulation entities: %d\n", entities.size()); - for (const auto& entity : entities) + if (!entities.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str()); + return; + } + + for (const auto& entity : entities.GetValue()) { AZ_Printf("SimulationInterfacesConsole", " - %s\n", entity.c_str()); } @@ -38,7 +43,6 @@ namespace SimulationInterfacesCommands SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::SetSimulationPaused, false); } - static void simulationinterfaces_Step(const AZ::ConsoleCommandContainer& arguments) { if (arguments.empty()) @@ -51,8 +55,6 @@ namespace SimulationInterfacesCommands SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::StepSimulation, steps); } - - static void simulationinterfaces_GetEntitiesSphere(const AZ::ConsoleCommandContainer& arguments) { float sphereShape = 10.f; @@ -67,10 +69,15 @@ namespace SimulationInterfacesCommands EntityFilters filter; filter.m_bounds_shape = AZStd::make_shared(sphereShape); - AZStd::vector entities; + AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); - AZ_Printf("SimulationInterfacesConsole", "Number of simulation entities: %d\n", entities.size()); - for (const auto& entity : entities) + if (!entities.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str()); + return; + } + + for (const auto& entity : entities.GetValue()) { AZ_Printf("SimulationInterfacesConsole", " - %s\n", entity.c_str()); } @@ -85,8 +92,14 @@ namespace SimulationInterfacesCommands } const AZStd::string entityName = arguments[0]; AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState %s\n", entityName.c_str()); - EntityState entityState; - SimulationEntityManagerRequestBus::BroadcastResult(entityState, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + AZ::Outcome entityStateResult; + SimulationEntityManagerRequestBus::BroadcastResult(entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + if (!entityStateResult.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().error_string.c_str()); + return; + } + const auto &entityState = entityStateResult.GetValue(); AZ_Printf("SimulationInterfacesConsole", "Entity %s\n", entityName.c_str()); AZ_Printf( "SimulationInterfacesConsole", @@ -124,17 +137,17 @@ namespace SimulationInterfacesCommands EntityState entityState{}; entityState.m_pose = AZ::Transform::CreateIdentity(); entityState.m_pose.SetTranslation(position); - bool isOk = false; + AZ::Outcome result; SimulationEntityManagerRequestBus::BroadcastResult( - isOk, &SimulationEntityManagerRequestBus::Events::SetEntityState, entityName, entityState); - if (isOk) - { - AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str()); - } - else + result, &SimulationEntityManagerRequestBus::Events::SetEntityState, entityName, entityState); + + if (!result.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Entity %s state NOT set\n", entityName.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().error_string.c_str()); + return; } + AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str()); + } static void simulationinterfaces_DeleteEntity(const AZ::ConsoleCommandContainer& arguments) @@ -145,26 +158,32 @@ namespace SimulationInterfacesCommands return; } const AZStd::string entityName = arguments[0]; - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_DeleteEntity %s\n", entityName.c_str()); - bool isOk = false; - SimulationEntityManagerRequestBus::BroadcastResult(isOk, &SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName); - if (isOk) - { - AZ_Printf("SimulationInterfacesConsole", "Entity %s deleted\n", entityName.c_str()); - } - else + DeletionCompletedCb cb = [](const AZ::Outcome& result) { - AZ_Printf("SimulationInterfacesConsole", "Entity %s NOT deleted\n", entityName.c_str()); - } + if (result.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "Entity deleted\n"); + } + else + { + AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().error_string.c_str()); + } + }; + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, cb); + } static void simulationinterfaces_GetSpawnables(const AZ::ConsoleCommandContainer& arguments) { AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetSpawnables\n"); - AZStd::vector spawnables; + AZ::Outcome spawnables; SimulationEntityManagerRequestBus::BroadcastResult(spawnables, &SimulationEntityManagerRequestBus::Events::GetSpawnables); - AZ_Printf("SimulationInterfacesConsole", "Number of spawnables: %d\n", spawnables.size()); - for (const auto& spawnable : spawnables) + if (!spawnables.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().error_string.c_str()); + return; + } + for (const auto& spawnable : spawnables.GetValue()) { AZ_Printf("SimulationInterfaces", " - %s\n", spawnable.m_uri.c_str()); } @@ -192,27 +211,28 @@ namespace SimulationInterfacesCommands AZStd::stof(AZStd::string(arguments[4])), AZStd::stof(AZStd::string(arguments[5])))); } - SimulationEntityManagerRequests::SpawnCompletedCb completedCb = [](const AZ::Outcome& name) + SpawnCompletedCb completedCb = [](const AZ::Outcome& result) { - if (name.IsSuccess()) + if (!result.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Entity %s spawned and registered\n", name.GetValue().c_str()); - } - else - { - AZ_Printf("SimulationInterfacesConsole", "Entity NOT spawned. Error : %s\n", name.GetError().c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().error_string.c_str()); + return; } + AZ_Printf("SimulationInterfacesConsole", "Entity spawned and registered : %s\n", result.GetValue().c_str()); + }; SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, completedCb); AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); } + AZ_CONSOLEFREEFUNC( + simulationinterfaces_GetEntities, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the scene."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_Pause, AZ::ConsoleFunctorFlags::DontReplicate, "Pause simulation."); AZ_CONSOLEFREEFUNC(simulationinterfaces_Resume, AZ::ConsoleFunctorFlags::DontReplicate, "Resume simulation."); AZ_CONSOLEFREEFUNC(simulationinterfaces_Step, AZ::ConsoleFunctorFlags::DontReplicate, "Step simulation."); - AZ_CONSOLEFREEFUNC( - simulationinterfaces_GetEntities, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the scene."); + AZ_CONSOLEFREEFUNC( simulationinterfaces_GetEntitiesSphere, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the radius."); AZ_CONSOLEFREEFUNC(simulationinterfaces_GetEntityState, AZ::ConsoleFunctorFlags::DontReplicate, "Get state of the entity."); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 54ac45bd2e..6763331847 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -254,8 +254,14 @@ namespace SimulationInterfaces } } - AZStd::vector SimulationEntitiesManager::GetEntities(const EntityFilters& filter) + AZ::Outcome SimulationEntitiesManager::GetEntities(const EntityFilters& filter) { + if (!filter.m_tags_filter.m_tags.empty()) + { + AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); + return AZ::Failure(FailedResult(ErrorCode::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); + } + const bool reFilter = !filter.m_filter.empty(); const bool shapeCastFilter = filter.m_bounds_shape != nullptr; @@ -280,7 +286,7 @@ namespace SimulationInterfaces if (m_physicsScenesHandle == AzPhysics::InvalidSceneHandle) { - return entities; + return AZ::Failure(FailedResult(ErrorCode::RESULT_OPERATION_FAILED, "Physics scene interface is not available.")); } AzPhysics::OverlapRequest request; @@ -304,103 +310,133 @@ namespace SimulationInterfaces const AZStd::vector prefilteredEntities = AZStd::move(entities); entities.clear(); const AZStd::regex regex(filter.m_filter); - if (regex.Valid()) + if (!regex.Valid()) { - AZStd::ranges::copy_if( - prefilteredEntities, - AZStd::back_inserter(entities), - [®ex](const AZStd::string& entityName) - { - return AZStd::regex_search(entityName, regex); - }); + AZ_Warning("SimulationInterfaces", false, "Invalid regex filter"); + return AZ::Failure(FailedResult(ErrorCode::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 entities; + return AZ::Success(entities); } - EntityState SimulationEntitiesManager::GetEntityState(const AZStd::string& name) + AZ::Outcome SimulationEntitiesManager::GetEntityState(const AZStd::string& name) { const auto findIt = m_simulatedEntityToEntityIdMap.find(name); - AZ_Error("SimulationInterfaces", findIt != m_simulatedEntityToEntityIdMap.end(), "Entity %s not found", name.c_str()); - if (findIt != m_simulatedEntityToEntityIdMap.end()) + if (findIt == m_simulatedEntityToEntityIdMap.end()) { - 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_Warning("SimulationInterfaces", findIt != m_simulatedEntityToEntityIdMap.end(), "Entity %s not found", name.c_str()); + return AZ::Failure(FailedResult(ErrorCode::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 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); - 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_twist_linear = entityTransformInv.TransformVector(linearVelocity); + entityState.m_twist_angular = entityTransformInv.TransformVector(angularVelocity); + return AZ::Success(entityState); + } - // transform linear and angular velocities to entity frame - AZ::Transform entityTransformInv = entityState.m_pose.GetInverse(); - entityState.m_twist_linear = entityTransformInv.TransformVector(linearVelocity); - entityState.m_twist_angular = entityTransformInv.TransformVector(angularVelocity); - return entityState; + AZ::Outcome SimulationEntitiesManager::GetEntitiesStates(const EntityFilters& filter) + { + if (!filter.m_tags_filter.m_tags.empty()) + { + AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); + return AZ::Failure(FailedResult(ErrorCode::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); + } + MultipleEntitiesStates entitiesStates; + const auto& entities = GetEntities(filter); + if (!entities.IsSuccess()) + { + return AZ::Failure(entities.GetError()); } - return {}; + 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; } - bool SimulationEntitiesManager::SetEntityState(const AZStd::string& name, const EntityState& state) + AZ::Outcome SimulationEntitiesManager::SetEntityState(const AZStd::string& name, const EntityState& state) { const auto findIt = m_simulatedEntityToEntityIdMap.find(name); - if (findIt != m_simulatedEntityToEntityIdMap.end()) + if (findIt == m_simulatedEntityToEntityIdMap.end()) { - const AZ::EntityId entityId = findIt->second; - AZ_Assert(entityId.IsValid(), "EntityId is not valid"); + AZ_Warning("SimulationInterfaces", false, "Entity %s not found", name.c_str()); + return AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found")); + } - // get entity and all descendants - AZStd::vector entityAndDescendants; - AZ::TransformBus::EventResult(entityAndDescendants, entityId, &AZ::TransformBus::Events::GetEntityAndAllDescendants); + 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); - if (state.m_pose.IsOrthogonal()) + if (state.m_pose.IsOrthogonal()) + { + // disable simulation for all entities + AZStd::map entityTransforms; + for (const auto& descendant : entityAndDescendants) { - // disable simulation for all entities - AZStd::map entityTransforms; - for (const auto& descendant : entityAndDescendants) - { - // get name - AZStd::string entityName = "Unknown"; - AZ::ComponentApplicationBus::BroadcastResult(entityName, &AZ::ComponentApplicationRequests::GetEntityName, descendant); - AZ_Printf("SimulationInterfaces", "Disable physics for entity %s\n", entityName.c_str()); - Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics); - } + // get name + AZStd::string entityName = "Unknown"; + AZ::ComponentApplicationBus::BroadcastResult(entityName, &AZ::ComponentApplicationRequests::GetEntityName, descendant); + AZ_Printf("SimulationInterfaces", "Disable physics for entity %s\n", entityName.c_str()); + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics); + } - AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose); + AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose); - for (const auto& descendant : entityAndDescendants) - { - Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::EnablePhysics); - Physics::RigidBodyRequestBus::Event( - descendant, &Physics::RigidBodyRequests::SetAngularVelocity, AZ::Vector3::CreateZero()); - Physics::RigidBodyRequestBus::Event( - descendant, &Physics::RigidBodyRequests::SetLinearVelocity, AZ::Vector3::CreateZero()); - } + for (const auto& descendant : entityAndDescendants) + { + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::EnablePhysics); + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::SetAngularVelocity, AZ::Vector3::CreateZero()); + Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::SetLinearVelocity, AZ::Vector3::CreateZero()); } - if (!state.m_twist_linear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twist_angular.IsZero(AZ::Constants::FloatEpsilon)) + } + + if (!state.m_twist_linear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twist_angular.IsZero(AZ::Constants::FloatEpsilon)) + { + // get rigid body + AzPhysics::RigidBody* rigidBody = nullptr; + Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody); + if (rigidBody != nullptr) { - // get rigid body - AzPhysics::RigidBody* rigidBody = nullptr; - Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody); - if (rigidBody != nullptr) - { - SetRigidBodyVelocities(rigidBody, state); - } + SetRigidBodyVelocities(rigidBody, state); } } - return false; + return AZ::Success(); } - bool SimulationEntitiesManager::DeleteEntity(const AZStd::string& name) + void SimulationEntitiesManager::DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) { const auto findIt = m_simulatedEntityToEntityIdMap.find(name); if (findIt == m_simulatedEntityToEntityIdMap.end()) { - return false; + completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found"))); } const AZ::EntityId entityId = findIt->second; @@ -414,12 +450,25 @@ namespace SimulationInterfaces if (m_spawnedTickets.find(ticketId) != m_spawnedTickets.end()) { // remove the ticket - m_spawnedTickets.erase(ticketId); + //m_spawnedTickets.erase(ticketId); + /// 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 { - AZ_Warning("SimulationInterfaces", false, "Entity %s was not spawned by this component, wont delete it", name.c_str()); - return false; + const auto msg = AZStd::string::format("Entity %s was not spawned by this component, wont delete it", name.c_str()); + completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_OPERATION_FAILED, msg))); } #ifdef POTENTIALY_UNSAFE if (findIt != m_simulatedEntityToEntityIdMap.end()) @@ -438,21 +487,9 @@ namespace SimulationInterfaces return true; } #endif - return false; - } - - AZStd::unordered_map SimulationEntitiesManager::GetEntitiesStates(const EntityFilters& filter) - { - AZStd::unordered_map entitiesStates; - const auto& entities = GetEntities(filter); - for (const auto& entity : entities) - { - entitiesStates.emplace(AZStd::make_pair(entity, GetEntityState(entity))); - } - return entitiesStates; } - AZStd::vector SimulationEntitiesManager::GetSpawnables() + AZ::Outcome SimulationEntitiesManager::GetSpawnables() { AZStd::vector spawnables; @@ -471,7 +508,7 @@ namespace SimulationInterfaces }; AZ::Data::AssetCatalogRequestBus::Broadcast(&AZ::Data::AssetCatalogRequests::EnumerateAssets, nullptr, enumCallback, nullptr); - return spawnables; + return AZ::Success(spawnables); } void SimulationEntitiesManager::SpawnEntity( @@ -501,7 +538,8 @@ namespace SimulationInterfaces AZ::Data::AssetManager::Instance().GetAsset(assetId, AZ::Data::AssetLoadBehavior::NoLoad); if (!spawnableAsset) { - completedCb(AZ::Failure("Failed to get spawnable asset - incorrect uri")); + const auto msg = AZStd::string::format("Spawnable asset %s not found", uri.c_str()); + completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, msg))); return; } @@ -536,18 +574,26 @@ namespace SimulationInterfaces } }; optionalArgs.m_completionCallback = - [this](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) + [this, uri](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) { // 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 AZFrameworrk::Pshysics::OnSimulationBodyAdded event was not 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 and remove the callback - spawnData->second.m_completedCb(AZ::Failure( - "Entity was not registered in simulation interface - no physics component or physics component is not enabled.")); + 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()); + AZ_Error("SimulationInterfaces", false, msg.c_str()); + spawnData->second.m_completedCb(msg); m_spawnCompletedCallbacks.erase(spawnData); } }; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index 25412f21b3..5bd7a3d26a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -37,12 +37,12 @@ namespace SimulationInterfaces protected: // SimulationEntityManagerRequestBus interface implementation - AZStd::vector GetEntities(const EntityFilters& filter) override; - EntityState GetEntityState(const AZStd::string& name) override; - AZStd::unordered_map GetEntitiesStates(const EntityFilters& filter) override; - bool SetEntityState(const AZStd::string& name, const EntityState& state) override; - bool DeleteEntity(const AZStd::string& name) override; - AZStd::vector GetSpawnables() override; + 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; + AZ::Outcome GetSpawnables() override; void SpawnEntity( const AZStd::string& name, const AZStd::string& uri, diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index 67233c9a45..a447fca24e 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -121,7 +121,6 @@ namespace SimulationInterfaces // install handler scene->RegisterSceneSimulationFinishHandler(m_simulationFinishEvent); SetSimulationPaused(false); - } } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index 8212e8e128..aa8b5d0ebf 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -18,9 +18,9 @@ #include namespace SimulationInterfaces { - class SimulationManager : - public AZ::Component, - protected SimulationManagerRequestBus::Handler + class SimulationManager + : public AZ::Component + , protected SimulationManagerRequestBus::Handler { public: AZ_COMPONENT_DECL(SimulationManager); @@ -39,11 +39,11 @@ namespace SimulationInterfaces void Init() override; void Activate() override; void Deactivate() override; + protected: void SetSimulationPaused(bool paused) override; void StepSimulation(AZ::u32 steps) override; uint32_t m_numberOfPhysicsSteps = 0; AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; - }; -} // namespace SimulationInterfaces \ No newline at end of file +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp index ec2ff7c74f..5c16f3c4dd 100644 --- a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -11,8 +11,8 @@ #include +#include #include -#include "Clients/SimulationManager.h" namespace SimulationInterfaces { diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp index c20439b84b..f99bfd2ed2 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp @@ -14,10 +14,7 @@ namespace SimulationInterfaces { AZ_COMPONENT_IMPL( - SimulationEntitiesManagerEditor, - "SimulationEntitiesManagerEditor", - SimulationEntitiesManagerEditorTypeId, - BaseSystemComponent); + SimulationEntitiesManagerEditor, "SimulationEntitiesManagerEditor", SimulationEntitiesManagerEditorTypeId, BaseSystemComponent); void SimulationEntitiesManagerEditor::Reflect(AZ::ReflectContext* context) { @@ -43,14 +40,12 @@ namespace SimulationInterfaces incompatible.push_back(AZ_CRC_CE("SimulationInterfacesEditorService")); } - void SimulationEntitiesManagerEditor::GetRequiredServices( - [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + void SimulationEntitiesManagerEditor::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { BaseSystemComponent::GetRequiredServices(required); } - void SimulationEntitiesManagerEditor::GetDependentServices( - [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + void SimulationEntitiesManagerEditor::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) { BaseSystemComponent::GetDependentServices(dependent); } diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h index cfec99885a..86b7ab3b70 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h @@ -1,10 +1,10 @@ /* -* 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 -* -*/ + * 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 @@ -14,30 +14,30 @@ namespace SimulationInterfaces { - /// System component for SimulationInterfaces editor - class SimulationManagerEditor - : public SimulationManager - , protected AzToolsFramework::EditorEvents::Bus::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; - }; + /// System component for SimulationInterfaces editor + class SimulationManagerEditor + : public SimulationManager + , protected AzToolsFramework::EditorEvents::Bus::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; + }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp index 17cb23c95c..69f1256de5 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp @@ -15,9 +15,11 @@ namespace UnitTest TEST_F(SimulationInterfaceTestFixture, EmptyScene) { using namespace SimulationInterfaces; - AZStd::vector entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - EXPECT_EQ(entities.size(), 0); + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities.IsSuccess()); + EXPECT_EQ(entities.GetValue().size(), 0); } TEST_F(SimulationInterfaceTestFixture, AddSimulatedEntityThenRemove) @@ -26,19 +28,25 @@ namespace UnitTest const AZ::EntityId entityId1 = CreateEntityWithStaticBodyComponent("Foo", AZ::Transform::CreateIdentity()); const AZ::EntityId entityId2 = CreateEntityWithStaticBodyComponent("Bar", AZ::Transform::CreateIdentity()); - AZStd::vector entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - ASSERT_EQ(entities.size(), 2); + AZ::Outcome entities; + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities.IsSuccess()); + ASSERT_EQ(entities.GetValue().size(), 2); DeleteEntity(entityId1); - AZStd::vector entities2; - SimulationEntityManagerRequestBus::BroadcastResult(entities2, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - EXPECT_EQ(entities2.size(), 1); + AZ::Outcome entities2; + SimulationEntityManagerRequestBus::BroadcastResult( + entities2, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities2.IsSuccess()); + EXPECT_EQ(entities2.GetValue().size(), 1); DeleteEntity(entityId2); - AZStd::vector entities3; - SimulationEntityManagerRequestBus::BroadcastResult(entities3, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - EXPECT_EQ(entities3.size(), 0); + AZ::Outcome entities3; + SimulationEntityManagerRequestBus::BroadcastResult( + entities3, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entities3.IsSuccess()); + EXPECT_EQ(entities3.GetValue().size(), 0); } TEST_F(SimulationInterfaceTestFixture, AddEntitiesWithDupName) @@ -47,11 +55,14 @@ namespace UnitTest const AZ::EntityId entityId1 = CreateEntityWithStaticBodyComponent("Bar1", AZ::Transform::CreateIdentity()); const AZ::EntityId entityId2 = CreateEntityWithStaticBodyComponent("Bar1", AZ::Transform::CreateIdentity()); - AZStd::vector entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - EXPECT_EQ(entities.size(), 2); - EXPECT_NE(entities[0], entities[1]); + 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); } @@ -60,7 +71,7 @@ namespace UnitTest { // 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."; + 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))); @@ -70,13 +81,14 @@ namespace UnitTest EntityFilters filter; filter.m_bounds_shape = AZStd::make_shared(2.0f); - AZStd::vector entities; + AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); auto* physicsSystem = AZ::Interface::Get(); physicsSystem->Simulate(1.0f / 60.0f); - ASSERT_EQ(entities.size(), 1); - EXPECT_EQ(entities.front(), "Inside"); + ASSERT_TRUE(entities.IsSuccess()); + ASSERT_EQ(entities.GetValue().size(), 1); + EXPECT_EQ(entities.GetValue().front(), "Inside"); DeleteEntity(entityId1); DeleteEntity(entityId2); @@ -93,11 +105,12 @@ namespace UnitTest EntityFilters filter; filter.m_filter = "Will.*"; - AZStd::vector entities; + AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); - ASSERT_EQ(entities.size(), 1); - EXPECT_EQ(entities.front(), "WillMatch"); + ASSERT_TRUE(entities.IsSuccess()); + ASSERT_EQ(entities.GetValue().size(), 1); + EXPECT_EQ(entities.GetValue().front(), "WillMatch"); DeleteEntity(entityId1); DeleteEntity(entityId2); @@ -115,10 +128,11 @@ namespace UnitTest EntityFilters filter; filter.m_filter = "[a-z"; - AZStd::vector entities; + AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); - EXPECT_EQ(entities.size(), 0); + EXPECT_FALSE(entities.IsSuccess()); + DeleteEntity(entityId1); DeleteEntity(entityId2); } @@ -132,16 +146,22 @@ namespace UnitTest CreateEntityWithStaticBodyComponent(entityName, AZ::Transform::CreateTranslation(AZ::Vector3(2.0f, 0.0f, 10.0f))); EntityFilters filter; - EntityState stateBefore; - SimulationEntityManagerRequestBus::BroadcastResult(stateBefore, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + 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); } - EntityState stateAfter; - SimulationEntityManagerRequestBus::BroadcastResult(stateAfter, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + 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 diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp index d389b3ba6f..3527def891 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -49,7 +49,6 @@ namespace UnitTest AZ_Assert(assetId.IsValid(), "Failed to get asset id for %s", TestSpawnable.c_str()); } - TEST_F(SimulationInterfaceTestFixture, SpawnAppTest) { // This is an integration test that runs the test application with the SimulationInterfaces gem enabled. @@ -61,7 +60,7 @@ namespace UnitTest constexpr AZStd::string_view uri = "product_asset:///sampleasset/testsimulationentity.spawnable"; constexpr AZStd::string_view entityNamespace = ""; AZStd::atomic_bool completed = false; - SimulationEntityManagerRequests::SpawnCompletedCb completedCb = [&](const AZ::Outcome& result) + SpawnCompletedCb completedCb = [&](const AZ::Outcome& result) { EXPECT_TRUE(result.IsSuccess()); completed = true; @@ -75,11 +74,14 @@ namespace UnitTest EXPECT_TRUE(completed); // list simulation entities - AZStd::vector entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + 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"; + ASSERT_FALSE(entities.empty()) << "Simulated Entities Empty"; const AZStd::string spawnedEntityName = entities.front(); printf("Spawned entity name %s\n", spawnedEntityName.c_str()); @@ -87,9 +89,12 @@ namespace UnitTest StepPhysics(100); // Get entity state, - AZStd::unordered_map entityStates; + AZ::Outcome entityStatesResult; SimulationEntityManagerRequestBus::BroadcastResult( - entityStates, &SimulationEntityManagerRequestBus::Events::GetEntitiesStates, EntityFilters()); + 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); @@ -99,8 +104,11 @@ namespace UnitTest // 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); + const EntityState newState = { AZ::Transform::CreateTranslation(newPosition), + AZ::Vector3::CreateZero(), + AZ::Vector3::CreateZero() }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SetEntityState, spawnedEntityName, newState); StepPhysics(); @@ -108,18 +116,27 @@ namespace UnitTest EntityFilters filter; filter.m_bounds_shape = AZStd::make_shared(2.0f); filter.m_bounds_pose = AZ::Transform::CreateTranslation(AZ::Vector3(1000.0f, 0.0f, 0.0f)); - AZStd::vector entitiesFiltered; - SimulationEntityManagerRequestBus::BroadcastResult(entitiesFiltered, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); - EXPECT_EQ(entitiesFiltered.size(), 1); + 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 - SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName); + DeletionCompletedCb deletionCompletedCb = [](const AZ::Outcome& result) + { + EXPECT_TRUE(result.IsSuccess()); + }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, deletionCompletedCb); TickApp(100); // list simulation entities after deletion, expect no simulation entities - AZStd::vector entities2; - SimulationEntityManagerRequestBus::BroadcastResult(entities2, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - EXPECT_EQ(entities2.size(), 0); + AZ::Outcome entitiesAfterDeletion; + SimulationEntityManagerRequestBus::BroadcastResult( + entitiesAfterDeletion, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + ASSERT_TRUE(entitiesAfterDeletion.IsSuccess()); + EXPECT_EQ(entitiesAfterDeletion.GetValue().size(), 0); } } // namespace UnitTest diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp index 9eec9d8e55..4dd1772bd9 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.cpp @@ -134,7 +134,6 @@ namespace UnitTest auto* physicsSystem = AZ::Interface::Get(); physicsSystem->Simulate(1.0f / 60.0f); } - } void SimulationInterfaceTestFixture::TickApp(int numTicks) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake index 329f4a5b87..555372701a 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake @@ -5,7 +5,9 @@ # set(FILES - Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h - Include/SimulationInterfaces/SimulationMangerRequestBus.h + Include/SimulationInterfaces/Result.h + Include/SimulationInterfaces/TagFilter.h + Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h Include/SimulationInterfaces/SimulationInterfacesTypeIds.h + Include/SimulationInterfaces/SimulationMangerRequestBus.h ) From 15a263342b76d2b64f923766bf355d88474f37b4 Mon Sep 17 00:00:00 2001 From: Patryk Antosz Date: Thu, 3 Apr 2025 17:01:31 +0200 Subject: [PATCH 04/35] Added ROS2 interface for SimulationInterfaces (#853) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added new gem with ROS 2 interface for simulation-interfaces * Integration with new SimulationInterfaces gem's API * Applied review suggestions --------- Signed-off-by: Patryk Antosz Co-authored-by: Michał Pełka --- Gems/SimulationInterfacesROS2/.clang-format | 61 +++++ Gems/SimulationInterfacesROS2/.gitignore | 0 Gems/SimulationInterfacesROS2/CMakeLists.txt | 6 + .../Code/CMakeLists.txt | 250 ++++++++++++++++++ .../SimulationInterfacesROS2TypeIds.h | 23 ++ .../Code/Platform/Android/PAL_android.cmake | 4 + .../simulationinterfacesros2_api_files.cmake | 3 + ...mulationinterfacesros2_private_files.cmake | 8 + ...imulationinterfacesros2_shared_files.cmake | 8 + .../Code/Platform/Linux/PAL_linux.cmake | 4 + .../simulationinterfacesros2_api_files.cmake | 3 + ...ationinterfacesros2_editor_api_files.cmake | 3 + ...mulationinterfacesros2_private_files.cmake | 8 + ...imulationinterfacesros2_shared_files.cmake | 8 + .../Code/Platform/Mac/PAL_mac.cmake | 4 + .../simulationinterfacesros2_api_files.cmake | 3 + ...ationinterfacesros2_editor_api_files.cmake | 3 + ...mulationinterfacesros2_private_files.cmake | 8 + ...imulationinterfacesros2_shared_files.cmake | 8 + .../Code/Platform/Windows/PAL_windows.cmake | 4 + .../simulationinterfacesros2_api_files.cmake | 3 + ...ationinterfacesros2_editor_api_files.cmake | 3 + ...mulationinterfacesros2_private_files.cmake | 8 + ...imulationinterfacesros2_shared_files.cmake | 8 + .../Code/Platform/iOS/PAL_ios.cmake | 4 + .../simulationinterfacesros2_api_files.cmake | 3 + ...mulationinterfacesros2_private_files.cmake | 8 + ...imulationinterfacesros2_shared_files.cmake | 8 + .../SimulationInterfacesROS2Module.cpp | 23 ++ ...imulationInterfacesROS2SystemComponent.cpp | 89 +++++++ .../SimulationInterfacesROS2SystemComponent.h | 62 +++++ .../Services/DeleteEntityServiceHandler.cpp | 58 ++++ .../Services/DeleteEntityServiceHandler.h | 37 +++ .../Services/GetEntitiesServiceHandler.cpp | 74 ++++++ .../Services/GetEntitiesServiceHandler.h | 35 +++ .../GetEntitiesStatesServiceHandler.cpp | 97 +++++++ .../GetEntitiesStatesServiceHandler.h | 35 +++ .../Services/GetEntityStateServiceHandler.cpp | 69 +++++ .../Services/GetEntityStateServiceHandler.h | 35 +++ .../Services/GetSpawnablesServiceHandler.cpp | 66 +++++ .../Services/GetSpawnablesServiceHandler.h | 35 +++ .../Services/SetEntityStateServiceHandler.cpp | 58 ++++ .../Services/SetEntityStateServiceHandler.h | 35 +++ .../Services/SpawnEntityServiceHandler.cpp | 68 +++++ .../Services/SpawnEntityServiceHandler.h | 37 +++ ...imulationInterfacesROS2ModuleInterface.cpp | 38 +++ .../SimulationInterfacesROS2ModuleInterface.h | 27 ++ .../SimulationInterfacesROS2EditorModule.cpp | 39 +++ ...ionInterfacesROS2EditorSystemComponent.cpp | 70 +++++ ...ationInterfacesROS2EditorSystemComponent.h | 42 +++ .../Code/Source/Utils/ServicesConfig.h | 20 ++ .../Code/Source/Utils/Utils.h | 56 ++++ .../Clients/SimulationInterfacesROS2Test.cpp | 4 + .../SimulationInterfacesROS2EditorTest.cpp | 4 + .../simulationinterfacesros2_api_files.cmake | 4 + ...ationinterfacesros2_editor_api_files.cmake | 4 + ...ninterfacesros2_editor_private_files.cmake | 5 + ...oninterfacesros2_editor_shared_files.cmake | 4 + ...ioninterfacesros2_editor_tests_files.cmake | 4 + ...mulationinterfacesros2_private_files.cmake | 27 ++ ...imulationinterfacesros2_shared_files.cmake | 4 + ...simulationinterfacesros2_tests_files.cmake | 4 + .../Registry/assetprocessor_settings.setreg | 18 ++ Gems/SimulationInterfacesROS2/gem.json | 28 ++ Gems/SimulationInterfacesROS2/preview.png | 3 + 65 files changed, 1782 insertions(+) create mode 100644 Gems/SimulationInterfacesROS2/.clang-format create mode 100644 Gems/SimulationInterfacesROS2/.gitignore create mode 100644 Gems/SimulationInterfacesROS2/CMakeLists.txt create mode 100644 Gems/SimulationInterfacesROS2/Code/CMakeLists.txt create mode 100644 Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake create mode 100644 Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg create mode 100644 Gems/SimulationInterfacesROS2/gem.json create mode 100644 Gems/SimulationInterfacesROS2/preview.png diff --git a/Gems/SimulationInterfacesROS2/.clang-format b/Gems/SimulationInterfacesROS2/.clang-format new file mode 100644 index 0000000000..fddf244031 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/.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/SimulationInterfacesROS2/.gitignore b/Gems/SimulationInterfacesROS2/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Gems/SimulationInterfacesROS2/CMakeLists.txt b/Gems/SimulationInterfacesROS2/CMakeLists.txt new file mode 100644 index 0000000000..c221acf540 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/CMakeLists.txt @@ -0,0 +1,6 @@ + +o3de_gem_setup("SimulationInterfacesROS2") + +ly_add_external_target_path(${CMAKE_CURRENT_SOURCE_DIR}/3rdParty) + +add_subdirectory(Code) diff --git a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt new file mode 100644 index 0000000000..4d8a9450e0 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt @@ -0,0 +1,250 @@ + +# 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/SimulationInterfacesROS2/Code/Platform/ or +# //Gems/SimulationInterfacesROS2/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_SIMULATIONINTERFACESROS2_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 + simulationinterfacesros2_api_files.cmake + ${pal_dir}/simulationinterfacesros2_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 + simulationinterfacesros2_private_files.cmake + ${pal_dir}/simulationinterfacesros2_private_files.cmake + TARGET_PROPERTIES + O3DE_PRIVATE_TARGET TRUE + INCLUDE_DIRECTORIES + PRIVATE + Include + Source + BUILD_DEPENDENCIES + PRIVATE + Gem::SimulationInterfaces.API + Gem::ROS2.Static + PUBLIC + AZ::AzCore + AZ::AzFramework +) + +target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces) + +# 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 + simulationinterfacesros2_shared_files.cmake + ${pal_dir}/simulationinterfacesros2_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/SimulationInterfacesROS2Module.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 + simulationinterfacesros2_editor_api_files.cmake + ${pal_dir}/simulationinterfacesros2_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 + simulationinterfacesros2_editor_private_files.cmake + TARGET_PROPERTIES + O3DE_PRIVATE_TARGET TRUE + INCLUDE_DIRECTORIES + PRIVATE + Include + Source + BUILD_DEPENDENCIES + PUBLIC + AZ::AzToolsFramework + ${gem_name}.Private.Object + ) + + ly_add_target( + NAME ${gem_name}.Editor GEM_MODULE + NAMESPACE Gem + AUTOMOC + FILES_CMAKE + simulationinterfacesros2_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/SimulationInterfacesROS2EditorModule.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_SIMULATIONINTERFACESROS2_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 + simulationinterfacesros2_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_SIMULATIONINTERFACESROS2_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 + simulationinterfacesros2_editor_tests_files.cmake + INCLUDE_DIRECTORIES + PRIVATE + Tests + Source + Include + BUILD_DEPENDENCIES + PRIVATE + AZ::AzTest + Gem::${gem_name}.Editor.Private.Object + ) + + # Add ${gem_name}.Editor.Tests to googletest + ly_add_googletest( + NAME Gem::${gem_name}.Editor.Tests + ) + endif() + endif() +endif() diff --git a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h b/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h new file mode 100644 index 0000000000..283daed823 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h @@ -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 + * + */ + +#pragma once + +namespace SimulationInterfacesROS2 +{ + // System Component TypeIds + inline constexpr const char* SimulationInterfacesROS2SystemComponentTypeId = "{9CD6E9FA-5C17-454C-B8FA-033DF572B160}"; + inline constexpr const char* SimulationInterfacesROS2EditorSystemComponentTypeId = "{AF5BE964-4B5F-49A4-A308-0B6077E5BB26}"; + + // Module derived classes TypeIds + inline constexpr const char* SimulationInterfacesROS2ModuleInterfaceTypeId = "{2F1ED7E1-6808-420D-939F-7D5C9CBFB3C9}"; + inline constexpr const char* SimulationInterfacesROS2ModuleTypeId = "{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* SimulationInterfacesROS2EditorModuleTypeId = SimulationInterfacesROS2ModuleTypeId; +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake new file mode 100644 index 0000000000..b561b79afb --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake @@ -0,0 +1,4 @@ + +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake new file mode 100644 index 0000000000..557799ce8d --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Android +# i.e. ../Source/Android/SimulationInterfacesROS2Android.cpp +# ../Source/Android/SimulationInterfacesROS2Android.h +# ../Include/Android/SimulationInterfacesROS2Android.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake new file mode 100644 index 0000000000..557799ce8d --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Android +# i.e. ../Source/Android/SimulationInterfacesROS2Android.cpp +# ../Source/Android/SimulationInterfacesROS2Android.h +# ../Include/Android/SimulationInterfacesROS2Android.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake new file mode 100644 index 0000000000..b561b79afb --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake @@ -0,0 +1,4 @@ + +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake new file mode 100644 index 0000000000..15f937f53a --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Linux +# i.e. ../Source/Linux/SimulationInterfacesROS2Linux.cpp +# ../Source/Linux/SimulationInterfacesROS2Linux.h +# ../Include/Linux/SimulationInterfacesROS2Linux.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake new file mode 100644 index 0000000000..15f937f53a --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Linux +# i.e. ../Source/Linux/SimulationInterfacesROS2Linux.cpp +# ../Source/Linux/SimulationInterfacesROS2Linux.h +# ../Include/Linux/SimulationInterfacesROS2Linux.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake new file mode 100644 index 0000000000..b561b79afb --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake @@ -0,0 +1,4 @@ + +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake new file mode 100644 index 0000000000..67b288e014 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Mac +# i.e. ../Source/Mac/SimulationInterfacesROS2Mac.cpp +# ../Source/Mac/SimulationInterfacesROS2Mac.h +# ../Include/Mac/SimulationInterfacesROS2Mac.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake new file mode 100644 index 0000000000..67b288e014 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Mac +# i.e. ../Source/Mac/SimulationInterfacesROS2Mac.cpp +# ../Source/Mac/SimulationInterfacesROS2Mac.h +# ../Include/Mac/SimulationInterfacesROS2Mac.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake new file mode 100644 index 0000000000..b561b79afb --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake @@ -0,0 +1,4 @@ + +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake new file mode 100644 index 0000000000..28928dc78c --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Windows +# i.e. ../Source/Windows/SimulationInterfacesROS2Windows.cpp +# ../Source/Windows/SimulationInterfacesROS2Windows.h +# ../Include/Windows/SimulationInterfacesROS2Windows.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake new file mode 100644 index 0000000000..28928dc78c --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for Windows +# i.e. ../Source/Windows/SimulationInterfacesROS2Windows.cpp +# ../Source/Windows/SimulationInterfacesROS2Windows.h +# ../Include/Windows/SimulationInterfacesROS2Windows.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake new file mode 100644 index 0000000000..b561b79afb --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake @@ -0,0 +1,4 @@ + +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake new file mode 100644 index 0000000000..f5526eeb37 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake @@ -0,0 +1,3 @@ + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake new file mode 100644 index 0000000000..2afd8fda21 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for iOS +# i.e. ../Source/iOS/SimulationInterfacesROS2iOS.cpp +# ../Source/iOS/SimulationInterfacesROS2iOS.h +# ../Include/iOS/SimulationInterfacesROS2iOS.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake new file mode 100644 index 0000000000..2afd8fda21 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake @@ -0,0 +1,8 @@ + +# Platform specific files for iOS +# i.e. ../Source/iOS/SimulationInterfacesROS2iOS.cpp +# ../Source/iOS/SimulationInterfacesROS2iOS.h +# ../Include/iOS/SimulationInterfacesROS2iOS.h + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp new file mode 100644 index 0000000000..6c19a010b3 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.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 "SimulationInterfacesROS2SystemComponent.h" +#include +#include + +namespace SimulationInterfacesROS2 +{ + class SimulationInterfacesROS2Module : public SimulationInterfacesROS2ModuleInterface + { + public: + AZ_RTTI(SimulationInterfacesROS2Module, SimulationInterfacesROS2ModuleTypeId, SimulationInterfacesROS2ModuleInterface); + AZ_CLASS_ALLOCATOR(SimulationInterfacesROS2Module, AZ::SystemAllocator); + }; +} // namespace SimulationInterfacesROS2 + +AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfacesROS2, SimulationInterfacesROS2::SimulationInterfacesROS2Module) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp new file mode 100644 index 0000000000..50194ae847 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -0,0 +1,89 @@ +/* + * 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 "SimulationInterfacesROS2SystemComponent.h" + +#include +#include + +#include + +namespace SimulationInterfacesROS2 +{ + AZ_COMPONENT_IMPL( + SimulationInterfacesROS2SystemComponent, "SimulationInterfacesROS2SystemComponent", SimulationInterfacesROS2SystemComponentTypeId); + + void SimulationInterfacesROS2SystemComponent::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + void SimulationInterfacesROS2SystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("SimulationInterfacesROS2Service")); + } + + void SimulationInterfacesROS2SystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("SimulationInterfacesROS2Service")); + } + + void SimulationInterfacesROS2SystemComponent::GetRequiredServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + } + + void SimulationInterfacesROS2SystemComponent::GetDependentServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + } + + void SimulationInterfacesROS2SystemComponent::Init() + { + } + + void SimulationInterfacesROS2SystemComponent::Activate() + { + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusConnect(); + } + + void SimulationInterfacesROS2SystemComponent::Deactivate() + { + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); + OnUnloadComplete(nullptr); + } + + void SimulationInterfacesROS2SystemComponent::OnLoadingComplete([[maybe_unused]] const char* levelName) + { + rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); + AZ_Assert(ros2Node, "ROS2 node is not available."); + + m_deleteEntityServiceHandler.emplace(ros2Node, DeleteEntityServiceName); + m_getEntitiesServiceHandler.emplace(ros2Node, GetEntitiesServiceName); + m_getSpawnablesServiceHandler.emplace(ros2Node, GetSpawnablesServiceName); + m_spawnEntityServiceHandler.emplace(ros2Node, SpawnEntityServiceName); + m_getEntitiesStatesServiceHandler.emplace(ros2Node, GetEntitiesStatesServiceName); + m_getEntityStateServiceHandler.emplace(ros2Node, GetEntityStateServiceName); + m_setEntityStateServiceHandler.emplace(ros2Node, SetEntityStateServiceName); + } + + void SimulationInterfacesROS2SystemComponent::OnUnloadComplete([[maybe_unused]] const char* levelName) + { + m_deleteEntityServiceHandler.reset(); + m_getEntitiesServiceHandler.reset(); + m_getSpawnablesServiceHandler.reset(); + m_spawnEntityServiceHandler.reset(); + m_getEntitiesStatesServiceHandler.reset(); + m_getEntityStateServiceHandler.reset(); + m_setEntityStateServiceHandler.reset(); + } + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h new file mode 100644 index 0000000000..40b1a0cd9e --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.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 "Services/DeleteEntityServiceHandler.h" +#include "Services/GetEntitiesServiceHandler.h" +#include "Services/GetEntitiesStatesServiceHandler.h" +#include "Services/GetEntityStateServiceHandler.h" +#include "Services/GetSpawnablesServiceHandler.h" +#include "Services/SetEntityStateServiceHandler.h" +#include "Services/SpawnEntityServiceHandler.h" +#include "Utils/ServicesConfig.h" + +namespace SimulationInterfacesROS2 +{ + class SimulationInterfacesROS2SystemComponent + : public AZ::Component + , public AzFramework::LevelSystemLifecycleNotificationBus::Handler + { + public: + AZ_COMPONENT_DECL(SimulationInterfacesROS2SystemComponent); + + 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); + + SimulationInterfacesROS2SystemComponent() = default; + ~SimulationInterfacesROS2SystemComponent() = default; + + protected: + // AZ::Component interface implementation + void Init() override; + void Activate() override; + void Deactivate() override; + + private: + // LevelSystemLifecycleNotificationBus::Handler overrides + void OnLoadingComplete([[maybe_unused]] const char* levelName) override; + void OnUnloadComplete([[maybe_unused]] const char* levelName) override; + + AZStd::optional m_deleteEntityServiceHandler; + AZStd::optional m_getEntitiesServiceHandler; + AZStd::optional m_getEntitiesStatesServiceHandler; + AZStd::optional m_getEntityStateServiceHandler; + AZStd::optional m_getSpawnablesServiceHandler; + AZStd::optional m_setEntityStateServiceHandler; + AZStd::optional m_spawnEntityServiceHandler; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp new file mode 100644 index 0000000000..9048f2e123 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -0,0 +1,58 @@ +/* + * 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 SimulationInterfacesROS2 +{ + DeleteEntityServiceHandler::DeleteEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_deleteEntityService = node->create_service( + serviceNameStr, + [this]( + const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) + { + HandleServiceRequest(service_handle, header, request); + }); + } + + DeleteEntityServiceHandler::~DeleteEntityServiceHandler() + { + if (m_deleteEntityService) + { + m_deleteEntityService.reset(); + } + } + + void DeleteEntityServiceHandler::HandleServiceRequest( + const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) + { + AZStd::string entityName = request->entity.c_str(); + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::DeleteEntity, + entityName, + [service_handle, header](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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + } + service_handle->send_response(*header, response); + }); + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h new file mode 100644 index 0000000000..9adb27834e --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h @@ -0,0 +1,37 @@ +/* + * 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 SimulationInterfacesROS2 +{ + + class DeleteEntityServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::DeleteEntity; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + using ServiceHandle = std::shared_ptr>; + + DeleteEntityServiceHandler() = delete; + DeleteEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~DeleteEntityServiceHandler(); + + void HandleServiceRequest( + const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request); + + private: + rclcpp::Service::SharedPtr m_deleteEntityService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp new file mode 100644 index 0000000000..0b4114da05 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -0,0 +1,74 @@ +/* + * 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 "Utils/Utils.h" +#include +#include + +namespace SimulationInterfacesROS2 +{ + GetEntitiesServiceHandler::GetEntitiesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_getEntitiesService = node->create_service( + serviceNameStr, + [this](const Request::SharedPtr request, Response::SharedPtr response) + { + *response = HandleServiceRequest(*request); + }); + } + + GetEntitiesServiceHandler::~GetEntitiesServiceHandler() + { + if (m_getEntitiesService) + { + m_getEntitiesService.reset(); + } + } + + GetEntitiesServiceHandler::Response GetEntitiesServiceHandler::HandleServiceRequest(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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h new file mode 100644 index 0000000000..b809d1ab6e --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.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 +#include +#include + +namespace SimulationInterfacesROS2 +{ + + class GetEntitiesServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::GetEntities; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + + GetEntitiesServiceHandler() = delete; + GetEntitiesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~GetEntitiesServiceHandler(); + + Response HandleServiceRequest(const Request& request); + + private: + rclcpp::Service::SharedPtr m_getEntitiesService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp new file mode 100644 index 0000000000..cc4aeb59da --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -0,0 +1,97 @@ +/* + * 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 "Utils/Utils.h" +#include +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + GetEntitiesStatesServiceHandler::GetEntitiesStatesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_getEntitiesStatesService = node->create_service( + serviceNameStr, + [this](const Request::SharedPtr request, Response::SharedPtr response) + { + *response = HandleServiceRequest(*request); + }); + } + + GetEntitiesStatesServiceHandler::~GetEntitiesStatesServiceHandler() + { + if (m_getEntitiesStatesService) + { + m_getEntitiesStatesService.reset(); + } + } + + GetEntitiesStatesServiceHandler::Response GetEntitiesStatesServiceHandler::HandleServiceRequest(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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + return response; + } + + const auto& multipleEntitiesStates = outcome.GetValue(); + std::vector stdEntities; + std::vector stdEntityStates; + + AZStd::transform( + multipleEntitiesStates.begin(), + multipleEntitiesStates.end(), + std::back_inserter(stdEntities), + [](const auto& pair) + { + return pair.first.c_str(); + }); + AZStd::transform( + multipleEntitiesStates.begin(), + multipleEntitiesStates.end(), + std::back_inserter(stdEntityStates), + [](const auto& pair) + { + const SimulationInterfaces::EntityState& entityState = pair.second; + simulation_interfaces::msg::EntityState simulationInterfaceEntityState; + std_msgs::msg::Header header; + header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); + simulationInterfaceEntityState.header = header; + simulationInterfaceEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); + simulationInterfaceEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); + simulationInterfaceEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); + return simulationInterfaceEntityState; + }); + response.entities = stdEntities; + response.states = stdEntityStates; + + return response; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h new file mode 100644 index 0000000000..4ac66be6bd --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 +#include +#include + +namespace SimulationInterfacesROS2 +{ + + class GetEntitiesStatesServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::GetEntitiesStates; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + + GetEntitiesStatesServiceHandler() = delete; + GetEntitiesStatesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~GetEntitiesStatesServiceHandler(); + + Response HandleServiceRequest(const Request& request); + + private: + rclcpp::Service::SharedPtr m_getEntitiesStatesService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp new file mode 100644 index 0000000000..b5f102b850 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -0,0 +1,69 @@ +/* + * 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 +#include + +namespace SimulationInterfacesROS2 +{ + + GetEntityStateServiceHandler::GetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_getEntityStateService = node->create_service( + serviceNameStr, + [this](const Request::SharedPtr request, Response::SharedPtr response) + { + *response = HandleServiceRequest(*request); + }); + } + + GetEntityStateServiceHandler::~GetEntityStateServiceHandler() + { + if (m_getEntityStateService) + { + m_getEntityStateService.reset(); + } + } + + GetEntityStateServiceHandler::Response GetEntityStateServiceHandler::HandleServiceRequest(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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + return response; + } + + const auto& entityState = outcome.GetValue(); + simulation_interfaces::msg::EntityState entityStateMsg; + std_msgs::msg::Header header; + header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); + entityStateMsg.header = header; + entityStateMsg.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); + entityStateMsg.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); + entityStateMsg.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); + + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + response.state = entityStateMsg; + return response; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h new file mode 100644 index 0000000000..b38bc26957 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.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 +#include +#include + +namespace SimulationInterfacesROS2 +{ + + class GetEntityStateServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::GetEntityState; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + + GetEntityStateServiceHandler() = delete; + GetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~GetEntityStateServiceHandler(); + + Response HandleServiceRequest(const Request& request); + + private: + rclcpp::Service::SharedPtr m_getEntityStateService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp new file mode 100644 index 0000000000..ad31ac81f6 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -0,0 +1,66 @@ +/* + * 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 SimulationInterfacesROS2 +{ + GetSpawnablesServiceHandler::GetSpawnablesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_getSpawnablesService = node->create_service( + serviceNameStr, + [this](const Request::SharedPtr request, Response::SharedPtr response) + { + *response = HandleServiceRequest(*request); + }); + } + + GetSpawnablesServiceHandler::~GetSpawnablesServiceHandler() + { + if (m_getSpawnablesService) + { + m_getSpawnablesService.reset(); + } + } + + GetSpawnablesServiceHandler::Response GetSpawnablesServiceHandler::HandleServiceRequest(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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + return response; + } + + const auto& spawnableList = outcome.GetValue(); + std::vector simSpawnables; + AZStd::transform( + spawnableList.begin(), + spawnableList.end(), + AZStd::back_inserter(simSpawnables), + [](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; + }); + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + response.spawnables = simSpawnables; + + return response; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h new file mode 100644 index 0000000000..f6243687de --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 +#include +#include + +namespace SimulationInterfacesROS2 +{ + + class GetSpawnablesServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::GetSpawnables; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + + GetSpawnablesServiceHandler() = delete; + GetSpawnablesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~GetSpawnablesServiceHandler(); + + Response HandleServiceRequest(const Request& request); + + private: + rclcpp::Service::SharedPtr m_getSpawnablesService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp new file mode 100644 index 0000000000..2b2dde728d --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -0,0 +1,58 @@ +/* + * 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 SimulationInterfacesROS2 +{ + SetEntityStateServiceHandler::SetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_setEntityStateService = node->create_service( + serviceNameStr, + [this](const Request::SharedPtr request, Response::SharedPtr response) + { + *response = HandleServiceRequest(*request); + }); + } + + SetEntityStateServiceHandler::~SetEntityStateServiceHandler() + { + if (m_setEntityStateService) + { + m_setEntityStateService.reset(); + } + } + + SetEntityStateServiceHandler::Response SetEntityStateServiceHandler::HandleServiceRequest(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_twist_angular = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.angular); + entityState.m_twist_linear = 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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + return response; + } + + return response; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h new file mode 100644 index 0000000000..a9cc032320 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 +#include +#include + +namespace SimulationInterfacesROS2 +{ + + class SetEntityStateServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::SetEntityState; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + + SetEntityStateServiceHandler() = delete; + SetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~SetEntityStateServiceHandler(); + + Response HandleServiceRequest(const Request& request); + + private: + rclcpp::Service::SharedPtr m_setEntityStateService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp new file mode 100644 index 0000000000..e9c275dab5 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -0,0 +1,68 @@ +/* + * 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 SimulationInterfacesROS2 +{ + SpawnEntityServiceHandler::SpawnEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_spawnEntityService = node->create_service( + serviceNameStr, + [this]( + const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) + { + HandleServiceRequest(service_handle, header, request); + }); + } + + SpawnEntityServiceHandler::~SpawnEntityServiceHandler() + { + if (m_spawnEntityService) + { + m_spawnEntityService.reset(); + } + } + + void SpawnEntityServiceHandler::HandleServiceRequest( + const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr 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()}; + const AZ::Transform initialPose = ROS2::ROS2Conversions::FromROS2Pose(request->initial_pose.pose); + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::SpawnEntity, + name, + uri, + entityNamespace, + initialPose, + [service_handle, header](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 = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + } + service_handle->send_response(*header, response); + }); + } + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h new file mode 100644 index 0000000000..e874e0fcf5 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h @@ -0,0 +1,37 @@ +/* + * 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 SimulationInterfacesROS2 +{ + + class SpawnEntityServiceHandler + { + public: + using ServiceType = simulation_interfaces::srv::SpawnEntity; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + using ServiceHandle = std::shared_ptr>; + + SpawnEntityServiceHandler() = delete; + SpawnEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~SpawnEntityServiceHandler(); + + void HandleServiceRequest( + const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request); + + private: + rclcpp::Service::SharedPtr m_spawnEntityService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp b/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp new file mode 100644 index 0000000000..d85b1133cd --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp @@ -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 + * + */ + +#include "SimulationInterfacesROS2ModuleInterface.h" +#include + +#include + +#include + +namespace SimulationInterfacesROS2 +{ + AZ_TYPE_INFO_WITH_NAME_IMPL( + SimulationInterfacesROS2ModuleInterface, "SimulationInterfacesROS2ModuleInterface", SimulationInterfacesROS2ModuleInterfaceTypeId); + AZ_RTTI_NO_TYPE_INFO_IMPL(SimulationInterfacesROS2ModuleInterface, AZ::Module); + AZ_CLASS_ALLOCATOR_IMPL(SimulationInterfacesROS2ModuleInterface, AZ::SystemAllocator); + + SimulationInterfacesROS2ModuleInterface::SimulationInterfacesROS2ModuleInterface() + { + m_descriptors.insert( + m_descriptors.end(), + { + SimulationInterfacesROS2SystemComponent::CreateDescriptor(), + }); + } + + AZ::ComponentTypeList SimulationInterfacesROS2ModuleInterface::GetRequiredSystemComponents() const + { + return AZ::ComponentTypeList{ + azrtti_typeid(), + }; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h b/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h new file mode 100644 index 0000000000..448e62490d --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h @@ -0,0 +1,27 @@ +/* + * 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 SimulationInterfacesROS2 +{ + class SimulationInterfacesROS2ModuleInterface : public AZ::Module + { + public: + AZ_TYPE_INFO_WITH_NAME_DECL(SimulationInterfacesROS2ModuleInterface) + AZ_RTTI_NO_TYPE_INFO_DECL() + AZ_CLASS_ALLOCATOR_DECL + + SimulationInterfacesROS2ModuleInterface(); + + AZ::ComponentTypeList GetRequiredSystemComponents() const override; + }; +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp new file mode 100644 index 0000000000..23c02f7e27 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp @@ -0,0 +1,39 @@ +/* + * 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 "SimulationInterfacesROS2EditorSystemComponent.h" +#include +#include + +namespace SimulationInterfacesROS2 +{ + class SimulationInterfacesROS2EditorModule : public SimulationInterfacesROS2ModuleInterface + { + public: + AZ_RTTI(SimulationInterfacesROS2EditorModule, SimulationInterfacesROS2EditorModuleTypeId, SimulationInterfacesROS2ModuleInterface); + AZ_CLASS_ALLOCATOR(SimulationInterfacesROS2EditorModule, AZ::SystemAllocator); + + SimulationInterfacesROS2EditorModule() + { + m_descriptors.insert( + m_descriptors.end(), + { + SimulationInterfacesROS2EditorSystemComponent::CreateDescriptor(), + }); + } + + AZ::ComponentTypeList GetRequiredSystemComponents() const override + { + return AZ::ComponentTypeList{ + azrtti_typeid(), + }; + } + }; +} // namespace SimulationInterfacesROS2 + +AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfacesROS2_Editor, SimulationInterfacesROS2::SimulationInterfacesROS2EditorModule) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp new file mode 100644 index 0000000000..ce3af330f6 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp @@ -0,0 +1,70 @@ +/* + * 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 "SimulationInterfacesROS2EditorSystemComponent.h" +#include + +#include + +namespace SimulationInterfacesROS2 +{ + AZ_COMPONENT_IMPL( + SimulationInterfacesROS2EditorSystemComponent, + "SimulationInterfacesROS2EditorSystemComponent", + SimulationInterfacesROS2EditorSystemComponentTypeId, + BaseSystemComponent); + + void SimulationInterfacesROS2EditorSystemComponent::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0); + } + } + + SimulationInterfacesROS2EditorSystemComponent::SimulationInterfacesROS2EditorSystemComponent() = default; + + SimulationInterfacesROS2EditorSystemComponent::~SimulationInterfacesROS2EditorSystemComponent() = default; + + void SimulationInterfacesROS2EditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + BaseSystemComponent::GetProvidedServices(provided); + provided.push_back(AZ_CRC_CE("SimulationInterfacesROS2EditorService")); + } + + void SimulationInterfacesROS2EditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + BaseSystemComponent::GetIncompatibleServices(incompatible); + incompatible.push_back(AZ_CRC_CE("SimulationInterfacesROS2EditorService")); + } + + void SimulationInterfacesROS2EditorSystemComponent::GetRequiredServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) + { + BaseSystemComponent::GetRequiredServices(required); + } + + void SimulationInterfacesROS2EditorSystemComponent::GetDependentServices( + [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + } + + void SimulationInterfacesROS2EditorSystemComponent::Activate() + { + SimulationInterfacesROS2SystemComponent::Activate(); + AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + } + + void SimulationInterfacesROS2EditorSystemComponent::Deactivate() + { + AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); + SimulationInterfacesROS2SystemComponent::Deactivate(); + } + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h new file mode 100644 index 0000000000..741da0e719 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.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 SimulationInterfacesROS2 +{ + /// System component for SimulationInterfacesROS2 editor + class SimulationInterfacesROS2EditorSystemComponent + : public SimulationInterfacesROS2SystemComponent + , protected AzToolsFramework::EditorEvents::Bus::Handler + { + using BaseSystemComponent = SimulationInterfacesROS2SystemComponent; + + public: + AZ_COMPONENT_DECL(SimulationInterfacesROS2EditorSystemComponent); + + static void Reflect(AZ::ReflectContext* context); + + SimulationInterfacesROS2EditorSystemComponent(); + ~SimulationInterfacesROS2EditorSystemComponent(); + + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h new file mode 100644 index 0000000000..b7bb883994 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.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 + +namespace SimulationInterfacesROS2 +{ + inline constexpr const char* DeleteEntityServiceName = "delete_entity"; + inline constexpr const char* GetEntitiesServiceName = "get_entities"; + inline constexpr const char* GetEntitiesStatesServiceName = "get_entities_states"; + inline constexpr const char* GetEntityStateServiceName = "get_entity_state"; + inline constexpr const char* GetSpawnablesServiceName = "get_spawnables"; + inline constexpr const char* SetEntityStateServiceName = "set_entity_state"; + inline constexpr const char* SpawnEntityServiceName = "spawn_entity"; +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h new file mode 100644 index 0000000000..d466ca76a1 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h @@ -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 + * + */ + +#pragma once + +#include "Services/GetEntityStateServiceHandler.h" +#include +#include +#include + +namespace SimulationInterfacesROS2::Utils +{ + template + AZ::Outcome GetEntityFiltersFromRequest(const RequestType& request) + { + SimulationInterfaces::EntityFilters filter; + filter.m_filter = request.filters.filter.c_str(); + uint8_t type = request.filters.bounds.type; + if (type == 1) // 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 upperRight = ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()); + const auto lowerLeft = ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.back()); + const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(lowerLeft, upperRight); + filter.m_bounds_shape = AZStd::make_shared(aabb.GetExtents()); + } + else if (type == 2) // TYPE_CONVEX_HULL + { + if (request.filters.bounds.points.size() < 3) + { + return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have exactly 2 points."); + } + filter.m_bounds_shape = AZStd::make_shared(); + } + else if (type == 3) // 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_bounds_shape = AZStd::make_shared(request.filters.bounds.points.back().x); + filter.m_bounds_pose = { ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()), + AZ::Quaternion::CreateIdentity(), + 1.0f }; + } + return AZ::Success(AZStd::move(filter)); + } +} // namespace SimulationInterfacesROS2::Utils diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp new file mode 100644 index 0000000000..274a990803 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp @@ -0,0 +1,4 @@ + +#include + +AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp new file mode 100644 index 0000000000..274a990803 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp @@ -0,0 +1,4 @@ + +#include + +AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake new file mode 100644 index 0000000000..5cbf0a08f0 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake @@ -0,0 +1,4 @@ + +set(FILES + Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake new file mode 100644 index 0000000000..8cd37a5cf0 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake @@ -0,0 +1,4 @@ + + +set(FILES +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake new file mode 100644 index 0000000000..b3187e11b6 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake @@ -0,0 +1,5 @@ + +set(FILES + Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp + Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake new file mode 100644 index 0000000000..fcc9fd1d42 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake @@ -0,0 +1,4 @@ + +set(FILES + Source/Tools/SimulationInterfacesROS2EditorModule.cpp +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake new file mode 100644 index 0000000000..2f93defce4 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake @@ -0,0 +1,4 @@ + +set(FILES + Tests/Tools/SimulationInterfacesROS2EditorTest.cpp +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake new file mode 100644 index 0000000000..799528026f --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -0,0 +1,27 @@ + +set(FILES + Source/SimulationInterfacesROS2ModuleInterface.cpp + Source/SimulationInterfacesROS2ModuleInterface.h + Source/Clients/SimulationInterfacesROS2SystemComponent.cpp + Source/Clients/SimulationInterfacesROS2SystemComponent.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/Utils/ServicesConfig.h + Source/Utils/Utils.h +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake new file mode 100644 index 0000000000..e710269e6e --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake @@ -0,0 +1,4 @@ + +set(FILES + Source/Clients/SimulationInterfacesROS2Module.cpp +) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake new file mode 100644 index 0000000000..095268dc57 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake @@ -0,0 +1,4 @@ + +set(FILES + Tests/Clients/SimulationInterfacesROS2Test.cpp +) diff --git a/Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg b/Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg new file mode 100644 index 0000000000..5c96cccb23 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg @@ -0,0 +1,18 @@ +{ + "Amazon": { + "AssetProcessor": { + "Settings": { + "ScanFolder SimulationInterfacesROS2/Assets": { + "watch": "@GEMROOT:SimulationInterfacesROS2@/Assets", + "recursive": 1, + "order": 101 + }, + "ScanFolder SimulationInterfacesROS2/Registry": { + "watch": "@GEMROOT:SimulationInterfacesROS2@/Registry", + "recursive": 1, + "order": 102 + } + } + } + } +} diff --git a/Gems/SimulationInterfacesROS2/gem.json b/Gems/SimulationInterfacesROS2/gem.json new file mode 100644 index 0000000000..61e19c816f --- /dev/null +++ b/Gems/SimulationInterfacesROS2/gem.json @@ -0,0 +1,28 @@ +{ + "gem_name": "SimulationInterfacesROS2", + "version": "1.0.0", + "display_name": "SimulationInterfacesROS2", + "license": "License used i.e. Apache-2.0 or MIT", + "license_url": "Link to the license web site i.e. https://opensource.org/licenses/Apache-2.0", + "origin": "The name of the originator or creator", + "origin_url": "The website for this Gem", + "type": "Code", + "summary": "A short description of this Gem", + "canonical_tags": [ + "Gem" + ], + "user_tags": [ + "SimulationInterfacesROS2" + ], + "platforms": [ + "" + ], + "icon_path": "preview.png", + "requirements": "Notice of any requirements for this Gem i.e. This requires X other gem", + "documentation_url": "Link to any documentation of your Gem", + "dependencies": [], + "repo_uri": "", + "compatible_engines": [], + "engine_api_dependencies": [], + "restricted": "SimulationInterfacesROS2" +} diff --git a/Gems/SimulationInterfacesROS2/preview.png b/Gems/SimulationInterfacesROS2/preview.png new file mode 100644 index 0000000000..b321ae48bc --- /dev/null +++ b/Gems/SimulationInterfacesROS2/preview.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:248e3ffe1fc9ffc02afb2ba8914e222a5a5d13ac45a48b98c95ee062e959a94c +size 4475 From 2b3689b4581605df592e925c622eed885973b7f9 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Fri, 4 Apr 2025 14:33:42 +0200 Subject: [PATCH 05/35] Added `get_simulation_features` to SimulationInterfaces and SimulationInterfacesROS2 (#856) * Added bus to get simulation features registered in SimulationInterfacesROS2 Gem * Added simulation features to SimulationInterfaces Gem with integration to SimulationInterfacesROS2 Gem * Added settings registry support for changing ROS 2 services names * Added missing reuqired service * Review chagnes except changing unordered_set Signed-off-by: Norbert Prokopiuk --------- Signed-off-by: Norbert Prokopiuk --- .../SimulationInterfaces/SimulationFeatures.h | 47 ++++++++++ .../SimulationFeaturesAggregatorRequestBus.h | 53 +++++++++++ .../SimulationInterfacesTypeIds.h | 4 + .../Clients/SimulationEntitiesManager.cpp | 20 +++- .../Clients/SimulationFeaturesAggregator.cpp | 92 ++++++++++++++++++ .../Clients/SimulationFeaturesAggregator.h | 54 +++++++++++ .../Code/Source/Clients/SimulationManager.cpp | 10 ++ .../SimulationInterfacesModuleInterface.cpp | 5 +- .../Tools/SimulationEntitiesManagerEditor.cpp | 2 + .../SimulationFeaturesAggregatorEditor.cpp | 67 +++++++++++++ .../SimulationFeaturesAggregatorEditor.h | 41 ++++++++ .../SimulationInterfacesEditorModule.cpp | 3 + .../Source/Tools/SimulationManagerEditor.cpp | 2 + .../Code/simulationinterfaces_api_files.cmake | 2 + ...ationinterfaces_editor_private_files.cmake | 2 + .../simulationinterfaces_private_files.cmake | 2 + .../Code/CMakeLists.txt | 1 + .../SimulationInterfacesROS2RequestBus.h | 44 +++++++++ .../SimulationInterfacesROS2TypeIds.h | 3 + ...imulationInterfacesROS2SystemComponent.cpp | 93 ++++++++++++++----- .../SimulationInterfacesROS2SystemComponent.h | 24 ++--- .../Services/DeleteEntityServiceHandler.cpp | 4 + .../Services/DeleteEntityServiceHandler.h | 5 +- .../Services/GetEntitiesServiceHandler.cpp | 8 ++ .../Services/GetEntitiesServiceHandler.h | 5 +- .../GetEntitiesStatesServiceHandler.cpp | 9 ++ .../GetEntitiesStatesServiceHandler.h | 5 +- .../Services/GetEntityStateServiceHandler.cpp | 5 + .../Services/GetEntityStateServiceHandler.h | 5 +- .../GetSimulationFeaturesServiceHandler.cpp | 79 ++++++++++++++++ .../GetSimulationFeaturesServiceHandler.h | 37 ++++++++ .../Services/GetSpawnablesServiceHandler.cpp | 5 + .../Services/GetSpawnablesServiceHandler.h | 5 +- .../Source/Services/ROS2HandlerBaseClass.h | 31 +++++++ .../Services/SetEntityStateServiceHandler.cpp | 5 + .../Services/SetEntityStateServiceHandler.h | 5 +- .../Services/SpawnEntityServiceHandler.cpp | 5 + .../Services/SpawnEntityServiceHandler.h | 5 +- ...ionInterfacesROS2EditorSystemComponent.cpp | 23 ++++- ...ationInterfacesROS2EditorSystemComponent.h | 5 + .../Code/Source/Utils/RegistryUtils.cpp | 24 +++++ .../Code/Source/Utils/RegistryUtils.h | 20 ++++ .../Code/Source/Utils/ServicesConfig.h | 23 +++-- ...mulationinterfacesros2_private_files.cmake | 7 +- 44 files changed, 833 insertions(+), 63 deletions(-) create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h create mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.cpp create mode 100644 Gems/SimulationInterfaces/Code/Source/Tools/SimulationFeaturesAggregatorEditor.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h new file mode 100644 index 0000000000..4ad2e72e8f --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.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 + +namespace SimulationInterfaces +{ + + //! Simulation Features copied from the simulation_interfaces + //! to avoid ros2 dependency + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + enum class SimulationFeatures : AZ::u8 + { + SPAWNING = 0, + DELETING = 1, + NAMED_POSES = 2, + POSE_BOUNDS = 3, + ENTITY_TAGS = 4, + ENTITY_BOUNDS = 5, + ENTITY_BOUNDS_BOX = 6, + ENTITY_BOUNDS_CONVEX = 7, + ENTITY_CATEGORIES = 8, + SPAWNING_RESOURCE_STRING = 9, + ENTITY_STATE_GETTING = 10, + ENTITY_STATE_SETTING = 11, + ENTITY_INFO_GETTING = 12, + ENTITY_INFO_SETTING = 13, + SPAWNABLES = 14, + SIMULATION_RESET = 20, + SIMULATION_RESET_TIME = 21, + SIMULATION_RESET_STATE = 22, + SIMULATION_RESET_SPAWNED = 23, + SIMULATION_STATE_GETTING = 24, + SIMULATION_STATE_SETTING = 25, + SIMULATION_STATE_PAUSE = 26, + STEP_SIMULATION_SINGLE = 31, + STEP_SIMULATION_MULTIPLE = 32, + STEP_SIMULATION_ACTION = 33 + }; +} // 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..be3920ffd6 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -0,0 +1,53 @@ +/* + * 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 "SimulationFeatures.h" +#include +#include +#include + +namespace SimulationInterfaces +{ + + 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 const AZStd::unordered_set& GetSimulationFeatures() const = 0; + + //! Method checks if feature with given id is available in the simulation + //! Method is extenstion to standard defined in simulation_interfaces + virtual bool HasFeature(SimulationFeatures feature) const = 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 index 7d165b3ed6..9a69035fa1 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h @@ -17,6 +17,9 @@ namespace SimulationInterfaces 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}"; @@ -27,5 +30,6 @@ namespace SimulationInterfaces // 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}"; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 6763331847..0f95418c22 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -10,14 +10,18 @@ #include +#include "Clients/SimulationFeaturesAggregator.h" #include "CommonUtilities.h" #include "ConsoleCommands.inl" +#include "SimulationInterfaces/SimulationFeatures.h" +#include "SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h" #include #include #include #include #include #include +#include #include #include #include @@ -68,11 +72,13 @@ namespace SimulationInterfaces 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() @@ -199,6 +205,18 @@ namespace SimulationInterfaces physicsSystem->RegisterSceneRemovedEvent(m_sceneRemovedHandler); SimulationEntityManagerRequestBus::Handler::BusConnect(); } + + SimulationFeaturesAggregatorRequestBus::Broadcast( + &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, + AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, + SimulationFeatures::ENTITY_BOUNDS_BOX, + SimulationFeatures::ENTITY_BOUNDS_CONVEX, + SimulationFeatures::ENTITY_CATEGORIES, + SimulationFeatures::ENTITY_STATE_GETTING, + SimulationFeatures::ENTITY_STATE_SETTING, + SimulationFeatures::DELETING, + SimulationFeatures::SPAWNABLES, + SimulationFeatures::SPAWNING }); } void SimulationEntitiesManager::Deactivate() @@ -450,7 +468,7 @@ namespace SimulationInterfaces if (m_spawnedTickets.find(ticketId) != m_spawnedTickets.end()) { // remove the ticket - //m_spawnedTickets.erase(ticketId); + // m_spawnedTickets.erase(ticketId); /// get spawner auto spawner = AZ::Interface::Get(); AZ_Assert(spawner, "SpawnableEntitiesDefinition is not available."); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp new file mode 100644 index 0000000000..4174dd8420 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.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 "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::Init() + { + } + + 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()); + } + + const AZStd::unordered_set& SimulationFeaturesAggregator::GetSimulationFeatures() const + { + return m_registeredFeatures; + } + + bool SimulationFeaturesAggregator::HasFeature(SimulationFeatures feature) const + { + 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..babf485919 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.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 +#include +#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 Init() override; + void Activate() override; + void Deactivate() override; + + protected: + // SimulationFeaturesAggregatorRequestBus overrides + void AddSimulationFeatures(const AZStd::unordered_set& features) override; + const AZStd::unordered_set& GetSimulationFeatures() const override; + bool HasFeature(SimulationFeatures feature) const override; + + private: + AZStd::unordered_set m_registeredFeatures; + }; +} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index a447fca24e..252071dbbc 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -7,10 +7,12 @@ */ #include "SimulationManager.h" +#include "SimulationInterfaces/SimulationFeatures.h" #include #include #include +#include #include namespace SimulationInterfaces @@ -39,10 +41,12 @@ namespace SimulationInterfaces 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")); } SimulationManager::SimulationManager() @@ -68,6 +72,12 @@ namespace SimulationInterfaces void SimulationManager::Activate() { SimulationManagerRequestBus::Handler::BusConnect(); + SimulationFeaturesAggregatorRequestBus::Broadcast( + &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, + AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_PAUSE, + SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE, + SimulationFeatures::STEP_SIMULATION_ACTION }); } void SimulationManager::Deactivate() diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp index 5c16f3c4dd..b9a69a3718 100644 --- a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -11,7 +11,8 @@ #include -#include +#include "Clients/SimulationFeaturesAggregator.h" +#include "Clients/SimulationManager.h" #include namespace SimulationInterfaces @@ -28,6 +29,7 @@ namespace SimulationInterfaces { SimulationEntitiesManager::CreateDescriptor(), SimulationManager::CreateDescriptor(), + SimulationFeaturesAggregator::CreateDescriptor(), }); } @@ -36,6 +38,7 @@ namespace SimulationInterfaces return AZ::ComponentTypeList{ azrtti_typeid(), azrtti_typeid(), + azrtti_typeid(), }; } } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp index f99bfd2ed2..5c276d7a11 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationEntitiesManagerEditor.cpp @@ -43,11 +43,13 @@ namespace SimulationInterfaces 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() 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 index 62eb71feb4..4311a8be9f 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp @@ -7,6 +7,7 @@ */ #include "SimulationEntitiesManagerEditor.h" +#include "SimulationFeaturesAggregatorEditor.h" #include "SimulationManagerEditor.h" #include #include @@ -25,6 +26,7 @@ namespace SimulationInterfaces { SimulationEntitiesManagerEditor::CreateDescriptor(), SimulationManagerEditor::CreateDescriptor(), + SimulationFeaturesAggregatorEditor::CreateDescriptor(), }); } @@ -36,6 +38,7 @@ namespace SimulationInterfaces { return AZ::ComponentTypeList{ azrtti_typeid(), + azrtti_typeid(), }; } }; diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp index d0366d858d..a99fb4f4c5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp @@ -42,11 +42,13 @@ namespace SimulationInterfaces 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() { diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake index 555372701a..69599001b1 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake @@ -10,4 +10,6 @@ set(FILES Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h Include/SimulationInterfaces/SimulationInterfacesTypeIds.h Include/SimulationInterfaces/SimulationMangerRequestBus.h + Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h + Include/SimulationInterfaces/SimulationFeatures.h ) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake index 2b0af4483c..ed94889986 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake @@ -9,4 +9,6 @@ set(FILES Source/Tools/SimulationManagerEditor.h Source/Tools/SimulationEntitiesManagerEditor.cpp Source/Tools/SimulationEntitiesManagerEditor.h + Source/Tools/SimulationFeaturesAggregatorEditor.cpp + Source/Tools/SimulationFeaturesAggregatorEditor.h ) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake index 8947b47d86..6c10d2ac8d 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake @@ -13,4 +13,6 @@ set(FILES Source/Clients/SimulationEntitiesManager.h Source/Clients/CommonUtilities.cpp Source/Clients/CommonUtilities.h + Source/Clients/SimulationFeaturesAggregator.cpp + Source/Clients/SimulationFeaturesAggregator.h ) diff --git a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt index 4d8a9450e0..d3be9c78d3 100644 --- a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt +++ b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt @@ -139,6 +139,7 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS) BUILD_DEPENDENCIES PUBLIC AZ::AzToolsFramework + Gem::ROS2.API ${gem_name}.Private.Object ) diff --git a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h b/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h new file mode 100644 index 0000000000..f5fa448ff2 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h @@ -0,0 +1,44 @@ +/* + * 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 SimulationInterfacesROS2 +{ + class SimulationInterfacesROS2Requests + { + public: + AZ_RTTI(SimulationInterfacesROS2Requests, SimulationInterfacesROS2RequestBusTypeId); + virtual ~SimulationInterfacesROS2Requests() = default; + + //! Returns set of simulation features available in SimulationInterfacesROS2 Gem + //! SimulationFeatures follows definition available at: + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + virtual AZStd::unordered_set GetSimulationFeatures() = 0; + }; + + class SimulationInterfacesROS2RequestBusTraits : public AZ::EBusTraits + { + public: + ////////////////////////////////////////////////////////////////////////// + // EBusTraits overrides + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + ////////////////////////////////////////////////////////////////////////// + }; + + using SimulationInterfacesROS2RequestBus = AZ::EBus; + using SimulationInterfacesROS2RequestBusInterface = AZ::Interface; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h b/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h index 283daed823..73246b9519 100644 --- a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h +++ b/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h @@ -20,4 +20,7 @@ namespace SimulationInterfacesROS2 // The Editor Module by default is mutually exclusive with the Client Module // so they use the Same TypeId inline constexpr const char* SimulationInterfacesROS2EditorModuleTypeId = SimulationInterfacesROS2ModuleTypeId; + + // API TypeIds + inline constexpr const char* SimulationInterfacesROS2RequestBusTypeId = "{00d08870-e329-4bd7-bb8c-f67fe369de92}"; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp index 50194ae847..e9186b58e0 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -7,14 +7,35 @@ */ #include "SimulationInterfacesROS2SystemComponent.h" +#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" +#include "Utils/ServicesConfig.h" +#include #include #include +#include "Utils/RegistryUtils.h" #include +#include namespace SimulationInterfacesROS2 { + + namespace + { + template + void RegisterInterface( + AZStd::unordered_map>& interfacesMap, + rclcpp::Node::SharedPtr ros2Node, + const AZStd::string& serviceType, + const AZStd::string& defaultName) + { + // add all known/implemented interfaces + auto serviceName = RegistryUtilities::GetServiceName(serviceType); + interfacesMap[serviceType] = AZStd::make_shared(ros2Node, serviceName.empty() ? defaultName : serviceName); + }; + } // namespace + AZ_COMPONENT_IMPL( SimulationInterfacesROS2SystemComponent, "SimulationInterfacesROS2SystemComponent", SimulationInterfacesROS2SystemComponentTypeId); @@ -39,6 +60,7 @@ namespace SimulationInterfacesROS2 void SimulationInterfacesROS2SystemComponent::GetRequiredServices( [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { + required.push_back(AZ_CRC_CE("ROS2Service")); } void SimulationInterfacesROS2SystemComponent::GetDependentServices( @@ -52,38 +74,61 @@ namespace SimulationInterfacesROS2 void SimulationInterfacesROS2SystemComponent::Activate() { - AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusConnect(); - } + SimulationInterfacesROS2RequestBus::Handler::BusConnect(); - void SimulationInterfacesROS2SystemComponent::Deactivate() - { - AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); - OnUnloadComplete(nullptr); + rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); + AZ_Assert(ros2Node, "ROS2 node is not available."); + + RegisterInterface( + m_availableRos2Interface, ros2Node, AZStd::string(DeleteEntityService), AZStd::string(DeleteEntityServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, ros2Node, AZStd::string(GetEntitiesService), AZStd::string(GetEntitiesServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, + ros2Node, + AZStd::string(GetEntitiesStatesService), + AZStd::string(GetEntitiesStatesServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, ros2Node, AZStd::string(GetEntityStateService), AZStd::string(GetEntityStateServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, ros2Node, AZStd::string(GetSpawnablesService), AZStd::string(GetSpawnablesServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, ros2Node, AZStd::string(SetEntityStateService), AZStd::string(SetEntityStateServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, ros2Node, AZStd::string(SpawnEntityService), AZStd::string(SpawnEntityServiceDefaultName)); + + RegisterInterface( + m_availableRos2Interface, + ros2Node, + AZStd::string(GetSimulationFeaturesService), + AZStd::string(GetSimulationFeaturesServiceDefaultName)); } - void SimulationInterfacesROS2SystemComponent::OnLoadingComplete([[maybe_unused]] const char* levelName) + void SimulationInterfacesROS2SystemComponent::Deactivate() { - rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); - AZ_Assert(ros2Node, "ROS2 node is not available."); + SimulationInterfacesROS2RequestBus::Handler::BusDisconnect(); - m_deleteEntityServiceHandler.emplace(ros2Node, DeleteEntityServiceName); - m_getEntitiesServiceHandler.emplace(ros2Node, GetEntitiesServiceName); - m_getSpawnablesServiceHandler.emplace(ros2Node, GetSpawnablesServiceName); - m_spawnEntityServiceHandler.emplace(ros2Node, SpawnEntityServiceName); - m_getEntitiesStatesServiceHandler.emplace(ros2Node, GetEntitiesStatesServiceName); - m_getEntityStateServiceHandler.emplace(ros2Node, GetEntityStateServiceName); - m_setEntityStateServiceHandler.emplace(ros2Node, SetEntityStateServiceName); + for (auto& [serviceType, service] : m_availableRos2Interface) + { + service.reset(); + } } - void SimulationInterfacesROS2SystemComponent::OnUnloadComplete([[maybe_unused]] const char* levelName) + AZStd::unordered_set SimulationInterfacesROS2SystemComponent::GetSimulationFeatures() { - m_deleteEntityServiceHandler.reset(); - m_getEntitiesServiceHandler.reset(); - m_getSpawnablesServiceHandler.reset(); - m_spawnEntityServiceHandler.reset(); - m_getEntitiesStatesServiceHandler.reset(); - m_getEntityStateServiceHandler.reset(); - m_setEntityStateServiceHandler.reset(); + AZStd::unordered_set result; + for (auto& [serviceType, serviceHandler] : m_availableRos2Interface) + { + auto features = serviceHandler->GetProvidedFeatures(); + result.insert(features.begin(), features.end()); + } + return result; } } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h index 40b1a0cd9e..c00da81d19 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h @@ -15,16 +15,23 @@ #include "Services/GetEntitiesServiceHandler.h" #include "Services/GetEntitiesStatesServiceHandler.h" #include "Services/GetEntityStateServiceHandler.h" +#include "Services/GetSimulationFeaturesServiceHandler.h" #include "Services/GetSpawnablesServiceHandler.h" +#include "Services/ROS2HandlerBaseClass.h" #include "Services/SetEntityStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" +#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" #include "Utils/ServicesConfig.h" +#include +#include +#include +#include namespace SimulationInterfacesROS2 { class SimulationInterfacesROS2SystemComponent : public AZ::Component - , public AzFramework::LevelSystemLifecycleNotificationBus::Handler + , public SimulationInterfacesROS2RequestBus::Handler { public: AZ_COMPONENT_DECL(SimulationInterfacesROS2SystemComponent); @@ -45,18 +52,11 @@ namespace SimulationInterfacesROS2 void Activate() override; void Deactivate() override; + // SimulationInterfacesROS2RequestBus override + AZStd::unordered_set GetSimulationFeatures() override; + private: - // LevelSystemLifecycleNotificationBus::Handler overrides - void OnLoadingComplete([[maybe_unused]] const char* levelName) override; - void OnUnloadComplete([[maybe_unused]] const char* levelName) override; - - AZStd::optional m_deleteEntityServiceHandler; - AZStd::optional m_getEntitiesServiceHandler; - AZStd::optional m_getEntitiesStatesServiceHandler; - AZStd::optional m_getEntityStateServiceHandler; - AZStd::optional m_getSpawnablesServiceHandler; - AZStd::optional m_setEntityStateServiceHandler; - AZStd::optional m_spawnEntityServiceHandler; + AZStd::unordered_map> m_availableRos2Interface; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp index 9048f2e123..35f4a5dbc6 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -31,6 +31,10 @@ namespace SimulationInterfacesROS2 m_deleteEntityService.reset(); } } + AZStd::unordered_set DeleteEntityServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::DELETING }; + } void DeleteEntityServiceHandler::HandleServiceRequest( const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h index 9adb27834e..69f9ee27ea 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class DeleteEntityServiceHandler + class DeleteEntityServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::DeleteEntity; @@ -26,7 +27,7 @@ namespace SimulationInterfacesROS2 DeleteEntityServiceHandler() = delete; DeleteEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~DeleteEntityServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; void HandleServiceRequest( const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request); diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp index 0b4114da05..533e90466b 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -32,6 +32,14 @@ namespace SimulationInterfacesROS2 } } + AZStd::unordered_set GetEntitiesServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, + SimulationFeatures::ENTITY_BOUNDS_BOX, + SimulationFeatures::ENTITY_BOUNDS_CONVEX, + SimulationFeatures::ENTITY_CATEGORIES }; + } + GetEntitiesServiceHandler::Response GetEntitiesServiceHandler::HandleServiceRequest(const Request& request) { AZ::Outcome outcome; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h index b809d1ab6e..1eb1a40ae4 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class GetEntitiesServiceHandler + class GetEntitiesServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::GetEntities; @@ -25,7 +26,7 @@ namespace SimulationInterfacesROS2 GetEntitiesServiceHandler() = delete; GetEntitiesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~GetEntitiesServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; Response HandleServiceRequest(const Request& request); private: diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index cc4aeb59da..bb97944e68 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -34,6 +34,15 @@ namespace SimulationInterfacesROS2 } } + 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 }; + } + GetEntitiesStatesServiceHandler::Response GetEntitiesStatesServiceHandler::HandleServiceRequest(const Request& request) { AZ::Outcome outcome; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h index 4ac66be6bd..47fca95ee5 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class GetEntitiesStatesServiceHandler + class GetEntitiesStatesServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::GetEntitiesStates; @@ -25,7 +26,7 @@ namespace SimulationInterfacesROS2 GetEntitiesStatesServiceHandler() = delete; GetEntitiesStatesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~GetEntitiesStatesServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; Response HandleServiceRequest(const Request& request); private: diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp index b5f102b850..d2c4c85943 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -34,6 +34,11 @@ namespace SimulationInterfacesROS2 } } + AZStd::unordered_set GetEntityStateServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_GETTING }; + } + GetEntityStateServiceHandler::Response GetEntityStateServiceHandler::HandleServiceRequest(const Request& request) { AZStd::string entityName = request.entity.c_str(); diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h index b38bc26957..88eb2ecced 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class GetEntityStateServiceHandler + class GetEntityStateServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::GetEntityState; @@ -25,7 +26,7 @@ namespace SimulationInterfacesROS2 GetEntityStateServiceHandler() = delete; GetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~GetEntityStateServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; Response HandleServiceRequest(const Request& request); private: diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp new file mode 100644 index 0000000000..5505de820d --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -0,0 +1,79 @@ +/* + * 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 "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" +#include +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + + GetSimulationFeaturesServiceHandler::GetSimulationFeaturesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) + { + const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); + m_getSimulationFeaturesService = node->create_service( + serviceNameStr, + [this](const Request::SharedPtr request, Response::SharedPtr response) + { + *response = HandleServiceRequest(*request); + }); + } + + GetSimulationFeaturesServiceHandler::~GetSimulationFeaturesServiceHandler() + { + if (m_getSimulationFeaturesService) + { + m_getSimulationFeaturesService.reset(); + } + } + + AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() + { + // standard doesn't define specific feature id for this service + return AZStd::unordered_set{}; + } + + GetSimulationFeaturesServiceHandler::Response GetSimulationFeaturesServiceHandler::HandleServiceRequest(const Request& request) + { + // call bus to get simulation features in SimulationInterfacesROS2 Gem side + AZStd::unordered_set ros2Interfaces; + SimulationInterfacesROS2RequestBus::BroadcastResult(ros2Interfaces, &SimulationInterfacesROS2Requests::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 + AZStd::unordered_set commonFeatures; + commonFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); + for (auto id : o3deInterfaces) + { + commonFeatures.insert(static_cast(id)); + } + + AZStd::vector idToRemove; + for (auto id : commonFeatures) + { + if (!(ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatures(id)))) + { + idToRemove.push_back(id); + } + } + for (auto id : idToRemove) + { + commonFeatures.erase(id); + } + + Response response; + response.features.features.insert(response.features.features.end(), commonFeatures.begin(), commonFeatures.end()); + return response; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h new file mode 100644 index 0000000000..688df9b65e --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -0,0 +1,37 @@ +/* + * 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/ROS2HandlerBaseClass.h" +#include +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + + class GetSimulationFeaturesServiceHandler : public ROS2HandlerBase + { + public: + using ServiceType = simulation_interfaces::srv::GetSimulatorFeatures; + using Request = ServiceType::Request; + using Response = ServiceType::Response; + using ServiceHandle = std::shared_ptr>; + + GetSimulationFeaturesServiceHandler() = delete; + GetSimulationFeaturesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); + ~GetSimulationFeaturesServiceHandler(); + AZStd::unordered_set GetProvidedFeatures() override; + Response HandleServiceRequest(const Request& request); + + private: + rclcpp::Service::SharedPtr m_getSimulationFeaturesService; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp index ad31ac81f6..19544a1348 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -30,6 +30,11 @@ namespace SimulationInterfacesROS2 } } + AZStd::unordered_set GetSpawnablesServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SPAWNABLES }; + } + GetSpawnablesServiceHandler::Response GetSpawnablesServiceHandler::HandleServiceRequest(const Request& request) { AZ::Outcome outcome; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h index f6243687de..52e2929f45 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class GetSpawnablesServiceHandler + class GetSpawnablesServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::GetSpawnables; @@ -25,7 +26,7 @@ namespace SimulationInterfacesROS2 GetSpawnablesServiceHandler() = delete; GetSpawnablesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~GetSpawnablesServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; Response HandleServiceRequest(const Request& request); private: diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h new file mode 100644 index 0000000000..c32e9f7006 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.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 +namespace SimulationInterfacesROS2 +{ + //! base for each ros2 handler, forces declaration of features provided by the handler + //! combined informations 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; + class ROS2HandlerBase + { + public: + virtual ~ROS2HandlerBase() = default; + + //! 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 + virtual AZStd::unordered_set GetProvidedFeatures() + { + return {}; + }; + }; +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp index 2b2dde728d..caef773b30 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -31,6 +31,11 @@ namespace SimulationInterfacesROS2 } } + AZStd::unordered_set SetEntityStateServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_SETTING }; + } + SetEntityStateServiceHandler::Response SetEntityStateServiceHandler::HandleServiceRequest(const Request& request) { AZ::Outcome outcome; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h index a9cc032320..56c1d21edd 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class SetEntityStateServiceHandler + class SetEntityStateServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::SetEntityState; @@ -25,7 +26,7 @@ namespace SimulationInterfacesROS2 SetEntityStateServiceHandler() = delete; SetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~SetEntityStateServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; Response HandleServiceRequest(const Request& request); private: diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp index e9c275dab5..3d76a6faac 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -34,6 +34,11 @@ namespace SimulationInterfacesROS2 } } + AZStd::unordered_set SpawnEntityServiceHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::SPAWNING }; + } + void SpawnEntityServiceHandler::HandleServiceRequest( const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) { diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h index e874e0fcf5..5431412692 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h @@ -8,6 +8,7 @@ #pragma once +#include "Services/ROS2HandlerBaseClass.h" #include #include #include @@ -15,7 +16,7 @@ namespace SimulationInterfacesROS2 { - class SpawnEntityServiceHandler + class SpawnEntityServiceHandler : public ROS2HandlerBase { public: using ServiceType = simulation_interfaces::srv::SpawnEntity; @@ -26,7 +27,7 @@ namespace SimulationInterfacesROS2 SpawnEntityServiceHandler() = delete; SpawnEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); ~SpawnEntityServiceHandler(); - + AZStd::unordered_set GetProvidedFeatures() override; void HandleServiceRequest( const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request); diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp index ce3af330f6..1c22a235d4 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp @@ -27,7 +27,23 @@ namespace SimulationInterfacesROS2 } } - SimulationInterfacesROS2EditorSystemComponent::SimulationInterfacesROS2EditorSystemComponent() = default; + SimulationInterfacesROS2EditorSystemComponent::SimulationInterfacesROS2EditorSystemComponent() + : m_nodeHandler( + [this](std::shared_ptr node) + { + if (!m_systemComponentActivated) + { + SimulationInterfacesROS2SystemComponent::Activate(); + m_systemComponentActivated = true; + } + else + { + SimulationInterfacesROS2SystemComponent::Deactivate(); + m_systemComponentActivated = false; + } + }) + { + } SimulationInterfacesROS2EditorSystemComponent::~SimulationInterfacesROS2EditorSystemComponent() = default; @@ -47,6 +63,7 @@ namespace SimulationInterfacesROS2 [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { BaseSystemComponent::GetRequiredServices(required); + required.push_back(AZ_CRC_CE("ROS2EditorService")); } void SimulationInterfacesROS2EditorSystemComponent::GetDependentServices( @@ -57,14 +74,14 @@ namespace SimulationInterfacesROS2 void SimulationInterfacesROS2EditorSystemComponent::Activate() { - SimulationInterfacesROS2SystemComponent::Activate(); AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + ROS2::ROS2Interface::Get()->ConnectOnNodeChanged(m_nodeHandler); } void SimulationInterfacesROS2EditorSystemComponent::Deactivate() { + m_nodeHandler.Disconnect(); AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); - SimulationInterfacesROS2SystemComponent::Deactivate(); } } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h index 741da0e719..0add61d690 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h @@ -10,7 +10,9 @@ #include +#include #include +#include namespace SimulationInterfacesROS2 { @@ -38,5 +40,8 @@ namespace SimulationInterfacesROS2 // AZ::Component void Activate() override; void Deactivate() override; + + bool m_systemComponentActivated = false; + ROS2::ROS2Requests::NodeChangedEvent::Handler m_nodeHandler; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp new file mode 100644 index 0000000000..4e4f38d384 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 SimulationInterfacesROS2::RegistryUtilities +{ + AZStd::string GetServiceName(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 SimulationInterfacesROS2::RegistryUtilities diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h new file mode 100644 index 0000000000..18392f00a3 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 SimulationInterfacesROS2::RegistryUtilities +{ + // prefix for settings registry related to ros2 topics names + inline constexpr const char* RegistryPrefix = "/SimulationInterfacesROS2"; + + //! Gets name of the service with defined type form settings registry + //! @return string with service name, if setting registry doesn't exits returns empty string + [[nodiscard]] AZStd::string GetServiceName(AZStd::string serviceType); +} // namespace SimulationInterfacesROS2::RegistryUtilities diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h index b7bb883994..121e3f0435 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h @@ -10,11 +10,20 @@ namespace SimulationInterfacesROS2 { - inline constexpr const char* DeleteEntityServiceName = "delete_entity"; - inline constexpr const char* GetEntitiesServiceName = "get_entities"; - inline constexpr const char* GetEntitiesStatesServiceName = "get_entities_states"; - inline constexpr const char* GetEntityStateServiceName = "get_entity_state"; - inline constexpr const char* GetSpawnablesServiceName = "get_spawnables"; - inline constexpr const char* SetEntityStateServiceName = "set_entity_state"; - inline constexpr const char* SpawnEntityServiceName = "spawn_entity"; + inline constexpr const char* DeleteEntityService = "DeleteEntity"; + inline constexpr const char* DeleteEntityServiceDefaultName = "delete_entity"; + inline constexpr const char* GetEntitiesService = "GetEntities"; + inline constexpr const char* GetEntitiesServiceDefaultName = "get_entities"; + inline constexpr const char* GetEntitiesStatesService = "GetEntitiesStates"; + inline constexpr const char* GetEntitiesStatesServiceDefaultName = "get_entities_states"; + inline constexpr const char* GetEntityStateService = "GetEntity"; + inline constexpr const char* GetEntityStateServiceDefaultName = "get_entity_state"; + inline constexpr const char* GetSpawnablesService = "GetSpawnables"; + inline constexpr const char* GetSpawnablesServiceDefaultName = "get_spawnables"; + inline constexpr const char* SetEntityStateService = "SetEntityState"; + inline constexpr const char* SetEntityStateServiceDefaultName = "set_entity_state"; + inline constexpr const char* SpawnEntityService = "SpawnEntity"; + inline constexpr const char* SpawnEntityServiceDefaultName = "spawn_entity"; + inline constexpr const char* GetSimulationFeaturesService = "GetSimulationFeatures"; + inline constexpr const char* GetSimulationFeaturesServiceDefaultName = "get_simulation_features"; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake index 799528026f..2735550b59 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -4,6 +4,7 @@ set(FILES Source/SimulationInterfacesROS2ModuleInterface.h Source/Clients/SimulationInterfacesROS2SystemComponent.cpp Source/Clients/SimulationInterfacesROS2SystemComponent.h + Source/Services/ROS2HandlerBaseClass.h Source/Services/GetEntitiesServiceHandler.cpp Source/Services/GetEntitiesServiceHandler.h Source/Services/DeleteEntityServiceHandler.cpp @@ -22,6 +23,10 @@ set(FILES Source/Services/GetSpawnablesServiceHandler.h Source/Services/SpawnEntityServiceHandler.cpp Source/Services/SpawnEntityServiceHandler.h + Source/Services/GetSimulationFeaturesServiceHandler.cpp + Source/Services/GetSimulationFeaturesServiceHandler.h + Source/Utils/RegistryUtils.cpp + Source/Utils/RegistryUtils.h Source/Utils/ServicesConfig.h - Source/Utils/Utils.h + Source/Utils/Utils.h ) From dbca5f37ec08792602655f0269a975ef2b5802ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 7 Apr 2025 13:12:17 +0100 Subject: [PATCH 06/35] Tests for SimulationInterfacesROS2 (#855) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Setting RPISystem as optional for ROS2SystemCameraComponent allowing to load ROS2 gem in test environment. * Added tests to ROS 2 services, some changes to the SimulationFeaturesAggregator * Adjust to review --------- Signed-off-by: Michał Pełka --- .../Camera/ROS2CameraSystemComponent.cpp | 6 +- .../Source/Camera/ROS2CameraSystemComponent.h | 1 + Gems/ROS2/Code/Source/ROS2ModuleInterface.h | 1 - .../SystemComponents/ROS2SystemComponent.cpp | 1 - .../SimulationFeaturesAggregatorRequestBus.h | 2 +- .../Clients/SimulationFeaturesAggregator.cpp | 2 +- .../Clients/SimulationFeaturesAggregator.h | 2 +- .../Code/CMakeLists.txt | 10 + .../Code/Platform/Linux/PAL_linux.cmake | 2 +- .../Code/Source/Utils/Utils.h | 17 +- .../Code/Tests/Tools/InterfacesTest.cpp | 410 ++++++++++++++++++ .../Tools/Mocks/SimulationEntityManagerMock.h | 31 ++ ...ionFeaturesAggregatorRequestsHandlerMock.h | 24 + .../SimulationInterfacesROS2EditorTest.cpp | 4 - ...ioninterfacesros2_editor_tests_files.cmake | 4 +- 15 files changed, 500 insertions(+), 17 deletions(-) create mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h delete mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp 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/ROS2ModuleInterface.h b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h index 899893987a..c57289745a 100644 --- a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h +++ b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h @@ -75,7 +75,6 @@ namespace ROS2 ROS2WheelOdometryComponent::CreateDescriptor(), ROS2FrameComponent::CreateDescriptor(), ROS2RobotControlComponent::CreateDescriptor(), - ROS2CameraSensorComponent::CreateDescriptor(), ROS2ImageEncodingConversionComponent::CreateDescriptor(), AckermannControlComponent::CreateDescriptor(), RigidBodyTwistControlComponent::CreateDescriptor(), 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/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h index be3920ffd6..eca8dcec13 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -29,7 +29,7 @@ namespace SimulationInterfaces //! Returns features available in the simulator, list follows definitions at //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg - virtual const AZStd::unordered_set& GetSimulationFeatures() const = 0; + virtual const AZStd::unordered_set GetSimulationFeatures() const = 0; //! Method checks if feature with given id is available in the simulation //! Method is extenstion to standard defined in simulation_interfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp index 4174dd8420..f73760c13e 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp @@ -79,7 +79,7 @@ namespace SimulationInterfaces m_registeredFeatures.insert(features.begin(), features.end()); } - const AZStd::unordered_set& SimulationFeaturesAggregator::GetSimulationFeatures() const + const AZStd::unordered_set SimulationFeaturesAggregator::GetSimulationFeatures() const { return m_registeredFeatures; } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h index babf485919..8fa8abe76c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h @@ -45,7 +45,7 @@ namespace SimulationInterfaces protected: // SimulationFeaturesAggregatorRequestBus overrides void AddSimulationFeatures(const AZStd::unordered_set& features) override; - const AZStd::unordered_set& GetSimulationFeatures() const override; + const AZStd::unordered_set GetSimulationFeatures() const override; bool HasFeature(SimulationFeatures feature) const override; private: diff --git a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt index d3be9c78d3..2179cae4fe 100644 --- a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt +++ b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt @@ -239,6 +239,16 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) 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 ) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake index b561b79afb..60cd4e35db 100644 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake +++ b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake @@ -1,4 +1,4 @@ set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) +set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED TRUE) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h index d466ca76a1..3522596b70 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h @@ -12,6 +12,7 @@ #include #include #include +#include namespace SimulationInterfacesROS2::Utils { @@ -21,18 +22,24 @@ namespace SimulationInterfacesROS2::Utils SimulationInterfaces::EntityFilters filter; filter.m_filter = request.filters.filter.c_str(); uint8_t type = request.filters.bounds.type; - if (type == 1) // TYPE_BOX + 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 upperRight = ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()); - const auto lowerLeft = ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.back()); + const auto &p1 = request.filters.bounds.points.front(); + const auto &p2 = request.filters.bounds.points.back(); + if (p1.x > p2.x || p1.y > p2.y || p1.z > p2.z) + { + return AZ::Failure("Invalid points! The first point should be lower than the second point."); + } + const auto upperRight = ROS2::ROS2Conversions::FromROS2Vector3(p2); + const auto lowerLeft = ROS2::ROS2Conversions::FromROS2Vector3(p1); const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(lowerLeft, upperRight); filter.m_bounds_shape = AZStd::make_shared(aabb.GetExtents()); } - else if (type == 2) // TYPE_CONVEX_HULL + else if (type == simulation_interfaces::msg::Bounds::TYPE_CONVEX_HULL) // TYPE_CONVEX_HULL { if (request.filters.bounds.points.size() < 3) { @@ -40,7 +47,7 @@ namespace SimulationInterfacesROS2::Utils } filter.m_bounds_shape = AZStd::make_shared(); } - else if (type == 3) // TYPE_SPHERE + else if (type == simulation_interfaces::msg::Bounds::TYPE_SPHERE) // TYPE_SPHERE { if (request.filters.bounds.points.size() != 2) { diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp new file mode 100644 index 0000000000..341b5c3ad4 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp @@ -0,0 +1,410 @@ + +/* + * 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 "Clients/SimulationInterfacesROS2SystemComponent.h" +#include "Mocks/SimulationEntityManagerMock.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h" + +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 SimulationInterfacesROS2; + AddActiveGems(AZStd::to_array({ "ROS2" })); + AddDynamicModulePaths({ "ROS2" }); + AddComponentDescriptors(AZStd::initializer_list{ + SimulationInterfacesROS2SystemComponent::CreateDescriptor(), + }); + AddRequiredComponents(AZStd::to_array({ SimulationInterfacesROS2SystemComponent::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() + { + 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 < 10; ++i) + { + app->Tick(); + } + } + }; + + //! 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()); + } + + //! 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); + SpinAppSome(); + + 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.error_code = SimulationInterfaces::ErrorCode::RESULT_NOT_FOUND; + failedResult.error_string = "FooBar not found."; + completedCb(AZ::Failure(failedResult)); + })); + + auto future = client->async_send_request(request); + SpinAppSome(); + + 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_bounds_shape, nullptr); + if (filter.m_bounds_shape) + { + EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Sphere); + Physics::SphereShapeConfiguration* sphereShape = azdynamic_cast(filter.m_bounds_shape.get()); + EXPECT_EQ(sphereShape->m_radius, 99.0f); + EXPECT_EQ(sphereShape->m_scale, AZ::Vector3(1.0f)); + + } + auto loc = filter.m_bounds_pose.GetTranslation(); + EXPECT_EQ(loc.GetX(), point1.GetX()); + EXPECT_EQ(loc.GetY(), point1.GetY()); + EXPECT_EQ(loc.GetZ(), point1.GetZ()); + + EXPECT_EQ(filter.m_filter, "FooBarFilter"); + return AZ::Success(EntityNameList{ "FooBar" }); + })); + + auto future = client->async_send_request(request); + SpinAppSome(); + + 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); + SpinAppSome(); + 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_bounds_shape, nullptr); + if (filter.m_bounds_shape) + { + EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Box); + Physics::BoxShapeConfiguration* boxShape = azdynamic_cast(filter.m_bounds_shape.get()); + EXPECT_EQ(boxShape->m_dimensions.GetX(), dims.GetX()); + EXPECT_EQ(boxShape->m_dimensions.GetY(), dims.GetY()); + EXPECT_EQ(boxShape->m_dimensions.GetZ(), dims.GetZ()); + + } + auto loc = filter.m_bounds_pose.GetTranslation(); + EXPECT_EQ(loc, AZ::Vector3::CreateZero()); + return AZ::Success(EntityNameList{ "FooBar" }); + })); + + auto future = client->async_send_request(request); + SpinAppSome(); + + 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); + SpinAppSome(); + + 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); + SpinAppSome(); + 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 { + SimulationFeatures::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); + SpinAppSome(); + 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."; + } + + + +} // 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/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h new file mode 100644 index 0000000000..ae32417825 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h @@ -0,0 +1,31 @@ +#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_METHOD0(GetSpawnables, AZ::Outcome()); + MOCK_METHOD5( + SpawnEntity, + void(const AZStd::string& name, const AZStd::string& uri, const AZStd::string& entityNamespace, const AZ::Transform& initialPose, SpawnCompletedCb completedCb)); + + }; +} diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h new file mode 100644 index 0000000000..94c8f263dc --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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(const AZStd::unordered_set, GetSimulationFeatures, (), (const, override)); + MOCK_METHOD(bool, HasFeature, (SimulationFeatures feature), (const, override)); + }; +} // namespace UnitTest diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp deleted file mode 100644 index 274a990803..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/SimulationInterfacesROS2EditorTest.cpp +++ /dev/null @@ -1,4 +0,0 @@ - -#include - -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake index 2f93defce4..e0a6a87474 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake @@ -1,4 +1,6 @@ set(FILES - Tests/Tools/SimulationInterfacesROS2EditorTest.cpp + Tests/Tools/Mocks/SimulationEntityManagerMock.h + Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h + Tests/Tools/InterfacesTest.cpp ) From e75f67b06da48d7d29706f375e22fd37b2e52c54 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Mon, 7 Apr 2025 15:07:07 +0200 Subject: [PATCH 07/35] SimulationInterfacesROS2 Gem clean up (#859) * Service handlers gathered in common container * Added possibility to delay service response * Interface file moved to dedicated directory * Fixed behaviour for delayed service response * Changes based on tests results --------- Signed-off-by: Norbert Prokopiuk --- ...imulationInterfacesROS2SystemComponent.cpp | 51 +++++----------- .../SimulationInterfacesROS2SystemComponent.h | 3 +- .../Code/Source/Interfaces/IROS2HandlerBase.h | 24 ++++++++ .../Services/DeleteEntityServiceHandler.cpp | 31 +++------- .../Services/DeleteEntityServiceHandler.h | 24 ++++---- .../Services/GetEntitiesServiceHandler.cpp | 22 +------ .../Services/GetEntitiesServiceHandler.h | 21 +++---- .../GetEntitiesStatesServiceHandler.cpp | 22 +------ .../GetEntitiesStatesServiceHandler.h | 20 ++++--- .../Services/GetEntityStateServiceHandler.cpp | 31 ++-------- .../Services/GetEntityStateServiceHandler.h | 21 +++---- .../GetSimulationFeaturesServiceHandler.cpp | 35 ++--------- .../GetSimulationFeaturesServiceHandler.h | 21 +++---- .../Services/GetSpawnablesServiceHandler.cpp | 21 +------ .../Services/GetSpawnablesServiceHandler.h | 20 ++++--- .../Source/Services/ROS2HandlerBaseClass.h | 60 ++++++++++++++++++- .../Services/SetEntityStateServiceHandler.cpp | 22 +------ .../Services/SetEntityStateServiceHandler.h | 20 ++++--- .../Services/SpawnEntityServiceHandler.cpp | 39 ++++-------- .../Services/SpawnEntityServiceHandler.h | 23 ++++--- .../Code/Source/Utils/ServicesConfig.h | 29 --------- .../Code/Source/Utils/Utils.h | 1 - ...mulationinterfacesros2_private_files.cmake | 2 +- 23 files changed, 226 insertions(+), 337 deletions(-) create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h delete mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp index e9186b58e0..ffb85d213d 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -7,8 +7,8 @@ */ #include "SimulationInterfacesROS2SystemComponent.h" +#include "Services/ROS2HandlerBaseClass.h" #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" -#include "Utils/ServicesConfig.h" #include #include @@ -25,14 +25,12 @@ namespace SimulationInterfacesROS2 { template void RegisterInterface( - AZStd::unordered_map>& interfacesMap, - rclcpp::Node::SharedPtr ros2Node, - const AZStd::string& serviceType, - const AZStd::string& defaultName) + AZStd::unordered_map>& interfacesMap, rclcpp::Node::SharedPtr ros2Node) { - // add all known/implemented interfaces - auto serviceName = RegistryUtilities::GetServiceName(serviceType); - interfacesMap[serviceType] = AZStd::make_shared(ros2Node, serviceName.empty() ? defaultName : serviceName); + AZStd::shared_ptr service = AZStd::make_shared(); + service->CreateService(ros2Node); + interfacesMap[service->GetTypeName()] = AZStd::move(service); + service.reset(); }; } // namespace @@ -79,35 +77,14 @@ namespace SimulationInterfacesROS2 rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); AZ_Assert(ros2Node, "ROS2 node is not available."); - RegisterInterface( - m_availableRos2Interface, ros2Node, AZStd::string(DeleteEntityService), AZStd::string(DeleteEntityServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, ros2Node, AZStd::string(GetEntitiesService), AZStd::string(GetEntitiesServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, - ros2Node, - AZStd::string(GetEntitiesStatesService), - AZStd::string(GetEntitiesStatesServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, ros2Node, AZStd::string(GetEntityStateService), AZStd::string(GetEntityStateServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, ros2Node, AZStd::string(GetSpawnablesService), AZStd::string(GetSpawnablesServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, ros2Node, AZStd::string(SetEntityStateService), AZStd::string(SetEntityStateServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, ros2Node, AZStd::string(SpawnEntityService), AZStd::string(SpawnEntityServiceDefaultName)); - - RegisterInterface( - m_availableRos2Interface, - ros2Node, - AZStd::string(GetSimulationFeaturesService), - AZStd::string(GetSimulationFeaturesServiceDefaultName)); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); } void SimulationInterfacesROS2SystemComponent::Deactivate() diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h index c00da81d19..8e773e7985 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h @@ -21,7 +21,6 @@ #include "Services/SetEntityStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" -#include "Utils/ServicesConfig.h" #include #include #include @@ -56,7 +55,7 @@ namespace SimulationInterfacesROS2 AZStd::unordered_set GetSimulationFeatures() override; private: - AZStd::unordered_map> m_availableRos2Interface; + AZStd::unordered_map> m_availableRos2Interface; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h b/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h new file mode 100644 index 0000000000..b0d5b6b495 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h @@ -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 + * + */ + +#pragma once +#include +#include + +namespace SimulationInterfacesROS2 +{ + // common interface to store all simulation feature ros2 handlers in common container + class IROS2HandlerBase + { + public: + virtual ~IROS2HandlerBase() = default; + virtual AZStd::unordered_set GetProvidedFeatures() = 0; + virtual AZStd::string_view GetTypeName() const = 0; + virtual AZStd::string_view GetDefaultName() const = 0; + }; +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp index 35f4a5dbc6..35050db756 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -7,43 +7,27 @@ */ #include "DeleteEntityServiceHandler.h" +#include #include #include namespace SimulationInterfacesROS2 { - DeleteEntityServiceHandler::DeleteEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_deleteEntityService = node->create_service( - serviceNameStr, - [this]( - const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) - { - HandleServiceRequest(service_handle, header, request); - }); - } - DeleteEntityServiceHandler::~DeleteEntityServiceHandler() - { - if (m_deleteEntityService) - { - m_deleteEntityService.reset(); - } - } AZStd::unordered_set DeleteEntityServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::DELETING }; } - void DeleteEntityServiceHandler::HandleServiceRequest( - const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) + AZStd::optional DeleteEntityServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { - AZStd::string entityName = request->entity.c_str(); + AZStd::string entityName = request.entity.c_str(); + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( &SimulationInterfaces::SimulationEntityManagerRequests::DeleteEntity, entityName, - [service_handle, header](const AZ::Outcome& outcome) + [this](const AZ::Outcome& outcome) { Response response; if (outcome.IsSuccess()) @@ -56,7 +40,8 @@ namespace SimulationInterfacesROS2 response.result.result = aznumeric_cast(failedResult.error_code); response.result.error_message = failedResult.error_string.c_str(); } - service_handle->send_response(*header, response); + SendResponse(response); }); + return AZStd::nullopt; } } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h index 69f9ee27ea..d9e66c8455 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h @@ -16,23 +16,23 @@ namespace SimulationInterfacesROS2 { - class DeleteEntityServiceHandler : public ROS2HandlerBase + class DeleteEntityServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::DeleteEntity; - using Request = ServiceType::Request; - using Response = ServiceType::Response; - using ServiceHandle = std::shared_ptr>; - - DeleteEntityServiceHandler() = delete; - DeleteEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~DeleteEntityServiceHandler(); + AZStd::string_view GetTypeName() const override + { + return "DeleteEntity"; + } + + AZStd::string_view GetDefaultName() const override + { + return "delete_entity"; + } AZStd::unordered_set GetProvidedFeatures() override; - void HandleServiceRequest( - const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_deleteEntityService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp index 533e90466b..a78ed7d043 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -13,25 +13,6 @@ namespace SimulationInterfacesROS2 { - GetEntitiesServiceHandler::GetEntitiesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_getEntitiesService = node->create_service( - serviceNameStr, - [this](const Request::SharedPtr request, Response::SharedPtr response) - { - *response = HandleServiceRequest(*request); - }); - } - - GetEntitiesServiceHandler::~GetEntitiesServiceHandler() - { - if (m_getEntitiesService) - { - m_getEntitiesService.reset(); - } - } - AZStd::unordered_set GetEntitiesServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, @@ -40,7 +21,8 @@ namespace SimulationInterfacesROS2 SimulationFeatures::ENTITY_CATEGORIES }; } - GetEntitiesServiceHandler::Response GetEntitiesServiceHandler::HandleServiceRequest(const Request& request) + AZStd::optional GetEntitiesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { AZ::Outcome outcome; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h index 1eb1a40ae4..ee322a9cef 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h @@ -15,22 +15,23 @@ namespace SimulationInterfacesROS2 { - - class GetEntitiesServiceHandler : public ROS2HandlerBase + class GetEntitiesServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::GetEntities; - using Request = ServiceType::Request; - using Response = ServiceType::Response; + AZStd::string_view GetTypeName() const override + { + return "GetEntities"; + } - GetEntitiesServiceHandler() = delete; - GetEntitiesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~GetEntitiesServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "get_entities"; + } AZStd::unordered_set GetProvidedFeatures() override; - Response HandleServiceRequest(const Request& request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_getEntitiesService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index bb97944e68..02baaabcbd 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -15,25 +15,6 @@ namespace SimulationInterfacesROS2 { - GetEntitiesStatesServiceHandler::GetEntitiesStatesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_getEntitiesStatesService = node->create_service( - serviceNameStr, - [this](const Request::SharedPtr request, Response::SharedPtr response) - { - *response = HandleServiceRequest(*request); - }); - } - - GetEntitiesStatesServiceHandler::~GetEntitiesStatesServiceHandler() - { - if (m_getEntitiesStatesService) - { - m_getEntitiesStatesService.reset(); - } - } - AZStd::unordered_set GetEntitiesStatesServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, @@ -43,7 +24,8 @@ namespace SimulationInterfacesROS2 SimulationFeatures::ENTITY_STATE_GETTING }; } - GetEntitiesStatesServiceHandler::Response GetEntitiesStatesServiceHandler::HandleServiceRequest(const Request& request) + AZStd::optional GetEntitiesStatesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { AZ::Outcome outcome; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h index 47fca95ee5..88ae42fe40 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h @@ -16,21 +16,23 @@ namespace SimulationInterfacesROS2 { - class GetEntitiesStatesServiceHandler : public ROS2HandlerBase + class GetEntitiesStatesServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::GetEntitiesStates; - using Request = ServiceType::Request; - using Response = ServiceType::Response; + AZStd::string_view GetTypeName() const override + { + return "GetEntitiesStates"; + } - GetEntitiesStatesServiceHandler() = delete; - GetEntitiesStatesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~GetEntitiesStatesServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "get_entities_states"; + } AZStd::unordered_set GetProvidedFeatures() override; - Response HandleServiceRequest(const Request& request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_getEntitiesStatesService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp index d2c4c85943..593c0a5a3c 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -7,39 +7,21 @@ */ #include "GetEntityStateServiceHandler.h" +#include #include #include #include -#include namespace SimulationInterfacesROS2 { - GetEntityStateServiceHandler::GetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_getEntityStateService = node->create_service( - serviceNameStr, - [this](const Request::SharedPtr request, Response::SharedPtr response) - { - *response = HandleServiceRequest(*request); - }); - } - - GetEntityStateServiceHandler::~GetEntityStateServiceHandler() - { - if (m_getEntityStateService) - { - m_getEntityStateService.reset(); - } - } - AZStd::unordered_set GetEntityStateServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_GETTING }; } - GetEntityStateServiceHandler::Response GetEntityStateServiceHandler::HandleServiceRequest(const Request& request) + AZStd::optional GetEntityStateServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { AZStd::string entityName = request.entity.c_str(); AZ::Outcome outcome; @@ -59,10 +41,8 @@ namespace SimulationInterfacesROS2 const auto& entityState = outcome.GetValue(); simulation_interfaces::msg::EntityState entityStateMsg; - std_msgs::msg::Header header; - header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); - header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); - entityStateMsg.header = header; + entityStateMsg.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + entityStateMsg.header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); entityStateMsg.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); entityStateMsg.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); entityStateMsg.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); @@ -71,4 +51,5 @@ namespace SimulationInterfacesROS2 response.state = entityStateMsg; return response; } + } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h index 88eb2ecced..05e3bd6e6b 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h @@ -16,21 +16,22 @@ namespace SimulationInterfacesROS2 { - class GetEntityStateServiceHandler : public ROS2HandlerBase + class GetEntityStateServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::GetEntityState; - using Request = ServiceType::Request; - using Response = ServiceType::Response; + AZStd::string_view GetTypeName() const override + { + return "GetEntityState"; + } - GetEntityStateServiceHandler() = delete; - GetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~GetEntityStateServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "get_entity_state"; + } AZStd::unordered_set GetProvidedFeatures() override; - Response HandleServiceRequest(const Request& request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_getEntityStateService; }; - } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 5505de820d..445987f125 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -16,32 +16,14 @@ namespace SimulationInterfacesROS2 { - GetSimulationFeaturesServiceHandler::GetSimulationFeaturesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_getSimulationFeaturesService = node->create_service( - serviceNameStr, - [this](const Request::SharedPtr request, Response::SharedPtr response) - { - *response = HandleServiceRequest(*request); - }); - } - - GetSimulationFeaturesServiceHandler::~GetSimulationFeaturesServiceHandler() - { - if (m_getSimulationFeaturesService) - { - m_getSimulationFeaturesService.reset(); - } - } - AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() { // standard doesn't define specific feature id for this service return AZStd::unordered_set{}; } - GetSimulationFeaturesServiceHandler::Response GetSimulationFeaturesServiceHandler::HandleServiceRequest(const Request& request) + AZStd::optional GetSimulationFeaturesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { // call bus to get simulation features in SimulationInterfacesROS2 Gem side AZStd::unordered_set ros2Interfaces; @@ -58,22 +40,15 @@ namespace SimulationInterfacesROS2 { commonFeatures.insert(static_cast(id)); } - - AZStd::vector idToRemove; + Response response; for (auto id : commonFeatures) { - if (!(ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatures(id)))) + if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatures(id))) { - idToRemove.push_back(id); + response.features.features.emplace_back(id); } } - for (auto id : idToRemove) - { - commonFeatures.erase(id); - } - Response response; - response.features.features.insert(response.features.features.end(), commonFeatures.begin(), commonFeatures.end()); return response; } } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h index 688df9b65e..fd7689e90e 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -16,22 +16,23 @@ namespace SimulationInterfacesROS2 { - class GetSimulationFeaturesServiceHandler : public ROS2HandlerBase + class GetSimulationFeaturesServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::GetSimulatorFeatures; - using Request = ServiceType::Request; - using Response = ServiceType::Response; - using ServiceHandle = std::shared_ptr>; + AZStd::string_view GetTypeName() const override + { + return "GetSimulationFeatures"; + } - GetSimulationFeaturesServiceHandler() = delete; - GetSimulationFeaturesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~GetSimulationFeaturesServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "get_simulation_features"; + } AZStd::unordered_set GetProvidedFeatures() override; - Response HandleServiceRequest(const Request& request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_getSimulationFeaturesService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp index 19544a1348..4c1969ad05 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -11,31 +11,14 @@ namespace SimulationInterfacesROS2 { - GetSpawnablesServiceHandler::GetSpawnablesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_getSpawnablesService = node->create_service( - serviceNameStr, - [this](const Request::SharedPtr request, Response::SharedPtr response) - { - *response = HandleServiceRequest(*request); - }); - } - - GetSpawnablesServiceHandler::~GetSpawnablesServiceHandler() - { - if (m_getSpawnablesService) - { - m_getSpawnablesService.reset(); - } - } AZStd::unordered_set GetSpawnablesServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::SPAWNABLES }; } - GetSpawnablesServiceHandler::Response GetSpawnablesServiceHandler::HandleServiceRequest(const Request& request) + AZStd::optional GetSpawnablesServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { AZ::Outcome outcome; SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h index 52e2929f45..35738f1f80 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h @@ -16,21 +16,23 @@ namespace SimulationInterfacesROS2 { - class GetSpawnablesServiceHandler : public ROS2HandlerBase + class GetSpawnablesServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::GetSpawnables; - using Request = ServiceType::Request; - using Response = ServiceType::Response; + AZStd::string_view GetTypeName() const override + { + return "GetSpawnables"; + } - GetSpawnablesServiceHandler() = delete; - GetSpawnablesServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~GetSpawnablesServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "get_spawnables"; + } AZStd::unordered_set GetProvidedFeatures() override; - Response HandleServiceRequest(const Request& request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_getSpawnablesService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h index c32e9f7006..5498ac8f06 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h @@ -8,24 +8,78 @@ #pragma once +#include "Interfaces/IROS2HandlerBase.h" +#include "Utils/RegistryUtils.h" #include +#include +#include +#include #include namespace SimulationInterfacesROS2 { - //! base for each ros2 handler, forces declaration of features provided by the handler + //! base for each ros2 service handler, forces declaration of features provided by the handler //! combined informations along all ROS 2 handlers gives information about simulation features //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures; - class ROS2HandlerBase + template + class ROS2HandlerBase : public virtual IROS2HandlerBase { public: + using Request = typename RosServiceType::Request; + using Response = typename RosServiceType::Response; + using ServiceHandle = std::shared_ptr>; virtual ~ROS2HandlerBase() = default; + void CreateService(rclcpp::Node::SharedPtr& node) + { + // get the service name from the type name + AZStd::string serviceName = RegistryUtilities::GetServiceName(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()); + } + }); + } + + 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 - virtual AZStd::unordered_set GetProvidedFeatures() + AZStd::unordered_set GetProvidedFeatures() override { return {}; }; + + private: + std::shared_ptr m_lastRequestHeader = nullptr; + ServiceHandle m_serviceHandle; }; + } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp index caef773b30..8926a4ce1b 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -7,36 +7,20 @@ */ #include "SetEntityStateServiceHandler.h" +#include #include #include namespace SimulationInterfacesROS2 { - SetEntityStateServiceHandler::SetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_setEntityStateService = node->create_service( - serviceNameStr, - [this](const Request::SharedPtr request, Response::SharedPtr response) - { - *response = HandleServiceRequest(*request); - }); - } - - SetEntityStateServiceHandler::~SetEntityStateServiceHandler() - { - if (m_setEntityStateService) - { - m_setEntityStateService.reset(); - } - } AZStd::unordered_set SetEntityStateServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_SETTING }; } - SetEntityStateServiceHandler::Response SetEntityStateServiceHandler::HandleServiceRequest(const Request& request) + AZStd::optional SetEntityStateServiceHandler::HandleServiceRequest( + const std::shared_ptr header, const Request& request) { AZ::Outcome outcome; AZStd::string entityName = request.entity.c_str(); diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h index 56c1d21edd..92471117ae 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h @@ -16,21 +16,23 @@ namespace SimulationInterfacesROS2 { - class SetEntityStateServiceHandler : public ROS2HandlerBase + class SetEntityStateServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::SetEntityState; - using Request = ServiceType::Request; - using Response = ServiceType::Response; + AZStd::string_view GetTypeName() const override + { + return "SetEntityState"; + } - SetEntityStateServiceHandler() = delete; - SetEntityStateServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~SetEntityStateServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "set_entity_state"; + } AZStd::unordered_set GetProvidedFeatures() override; - Response HandleServiceRequest(const Request& request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_setEntityStateService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp index 3d76a6faac..ca130ae78a 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -7,6 +7,8 @@ */ #include "SpawnEntityServiceHandler.h" +#include "Services/ROS2HandlerBaseClass.h" +#include #include #include #include @@ -14,45 +16,27 @@ namespace SimulationInterfacesROS2 { - SpawnEntityServiceHandler::SpawnEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName) - { - const std::string serviceNameStr(std::string_view(serviceName.data(), serviceName.size())); - m_spawnEntityService = node->create_service( - serviceNameStr, - [this]( - const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) - { - HandleServiceRequest(service_handle, header, request); - }); - } - - SpawnEntityServiceHandler::~SpawnEntityServiceHandler() - { - if (m_spawnEntityService) - { - m_spawnEntityService.reset(); - } - } AZStd::unordered_set SpawnEntityServiceHandler::GetProvidedFeatures() { return AZStd::unordered_set{ SimulationFeatures::SPAWNING }; } - void SpawnEntityServiceHandler::HandleServiceRequest( - const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request) + 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()}; - const AZ::Transform initialPose = ROS2::ROS2Conversions::FromROS2Pose(request->initial_pose.pose); + 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() }; + const AZ::Transform initialPose = ROS2::ROS2Conversions::FromROS2Pose(request.initial_pose.pose); + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( &SimulationInterfaces::SimulationEntityManagerRequests::SpawnEntity, name, uri, entityNamespace, initialPose, - [service_handle, header](const AZ::Outcome& outcome) + [this](const AZ::Outcome& outcome) { Response response; if (outcome.IsSuccess()) @@ -66,8 +50,9 @@ namespace SimulationInterfacesROS2 response.result.result = aznumeric_cast(failedResult.error_code); response.result.error_message = failedResult.error_string.c_str(); } - service_handle->send_response(*header, response); + SendResponse(response); }); + return AZStd::nullopt; } } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h index 5431412692..138be818da 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h @@ -15,24 +15,23 @@ namespace SimulationInterfacesROS2 { - - class SpawnEntityServiceHandler : public ROS2HandlerBase + class SpawnEntityServiceHandler : public ROS2HandlerBase { public: - using ServiceType = simulation_interfaces::srv::SpawnEntity; - using Request = ServiceType::Request; - using Response = ServiceType::Response; - using ServiceHandle = std::shared_ptr>; + AZStd::string_view GetTypeName() const override + { + return "SpawnEntity"; + } - SpawnEntityServiceHandler() = delete; - SpawnEntityServiceHandler(rclcpp::Node::SharedPtr& node, AZStd::string_view serviceName); - ~SpawnEntityServiceHandler(); + AZStd::string_view GetDefaultName() const override + { + return "spawn_entity"; + } AZStd::unordered_set GetProvidedFeatures() override; - void HandleServiceRequest( - const ServiceHandle service_handle, const std::shared_ptr header, const std::shared_ptr request); + + AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: - rclcpp::Service::SharedPtr m_spawnEntityService; }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h deleted file mode 100644 index 121e3f0435..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/ServicesConfig.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * 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 SimulationInterfacesROS2 -{ - inline constexpr const char* DeleteEntityService = "DeleteEntity"; - inline constexpr const char* DeleteEntityServiceDefaultName = "delete_entity"; - inline constexpr const char* GetEntitiesService = "GetEntities"; - inline constexpr const char* GetEntitiesServiceDefaultName = "get_entities"; - inline constexpr const char* GetEntitiesStatesService = "GetEntitiesStates"; - inline constexpr const char* GetEntitiesStatesServiceDefaultName = "get_entities_states"; - inline constexpr const char* GetEntityStateService = "GetEntity"; - inline constexpr const char* GetEntityStateServiceDefaultName = "get_entity_state"; - inline constexpr const char* GetSpawnablesService = "GetSpawnables"; - inline constexpr const char* GetSpawnablesServiceDefaultName = "get_spawnables"; - inline constexpr const char* SetEntityStateService = "SetEntityState"; - inline constexpr const char* SetEntityStateServiceDefaultName = "set_entity_state"; - inline constexpr const char* SpawnEntityService = "SpawnEntity"; - inline constexpr const char* SpawnEntityServiceDefaultName = "spawn_entity"; - inline constexpr const char* GetSimulationFeaturesService = "GetSimulationFeatures"; - inline constexpr const char* GetSimulationFeaturesServiceDefaultName = "get_simulation_features"; -} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h index 3522596b70..e5a8f41905 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h @@ -8,7 +8,6 @@ #pragma once -#include "Services/GetEntityStateServiceHandler.h" #include #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake index 2735550b59..d35354c036 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -4,6 +4,7 @@ set(FILES Source/SimulationInterfacesROS2ModuleInterface.h Source/Clients/SimulationInterfacesROS2SystemComponent.cpp Source/Clients/SimulationInterfacesROS2SystemComponent.h + Source/Interfaces/IROS2HandlerBase.h Source/Services/ROS2HandlerBaseClass.h Source/Services/GetEntitiesServiceHandler.cpp Source/Services/GetEntitiesServiceHandler.h @@ -27,6 +28,5 @@ set(FILES Source/Services/GetSimulationFeaturesServiceHandler.h Source/Utils/RegistryUtils.cpp Source/Utils/RegistryUtils.h - Source/Utils/ServicesConfig.h Source/Utils/Utils.h ) From ffb9a538d4760a0ce618e14ae8d269f9e9f5a62c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 7 Apr 2025 14:17:00 +0100 Subject: [PATCH 08/35] Adjust state applying (#858) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Code/Source/Clients/SimulationEntitiesManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 0f95418c22..1fc11e0144 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -45,7 +45,7 @@ namespace SimulationInterfaces { // get transform AZ::Vector3 linearVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_linear); - rigidBody->SetAngularVelocity(linearVelWorld); + rigidBody->SetLinearVelocity(linearVelWorld); } } From a6561a332e9c02ba71cf72e1dc182c29485f4ab2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 7 Apr 2025 16:29:41 +0100 Subject: [PATCH 09/35] Fix undefined behavior in removal already removed simulated entities. (#860) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Code/Source/Clients/SimulationEntitiesManager.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 1fc11e0144..ece2eb33f3 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -455,6 +455,7 @@ namespace SimulationInterfaces if (findIt == m_simulatedEntityToEntityIdMap.end()) { completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found"))); + return; } const AZ::EntityId entityId = findIt->second; @@ -463,6 +464,12 @@ namespace SimulationInterfaces 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(ErrorCode::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()) From 592e76ca037917595168148fac932fed999700f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 8 Apr 2025 16:23:50 +0100 Subject: [PATCH 10/35] Add allow renaming option to Simulation Interface spawner (#863) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add allow renaming option to Simulation Interface spawner * Applied review --------- Signed-off-by: Michał Pełka --- .../SimulationEntityManagerRequestBus.h | 2 +- .../Code/Source/Clients/ConsoleCommands.inl | 3 +- .../Clients/SimulationEntitiesManager.cpp | 29 ++++++++++++++----- .../Clients/SimulationEntitiesManager.h | 1 + .../Tests/Tools/SimulationIterfaceAppTest.cpp | 15 ++++++++-- .../Services/SpawnEntityServiceHandler.cpp | 2 +- .../Tools/Mocks/SimulationEntityManagerMock.h | 4 +-- 7 files changed, 42 insertions(+), 14 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index d07941b206..19a5bbb46c 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -86,12 +86,12 @@ namespace SimulationInterfaces //! 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 is 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; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl index ef5ecbaf85..a6faca84ef 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl @@ -221,7 +221,8 @@ namespace SimulationInterfacesCommands AZ_Printf("SimulationInterfacesConsole", "Entity spawned and registered : %s\n", result.GetValue().c_str()); }; - SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, completedCb); + constexpr bool allowRename = true; + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb); AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index ece2eb33f3..c75a324e3d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -541,12 +541,33 @@ namespace SimulationInterfaces 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(ErrorCode::RESULT_INCORRECT_STATE, msg))); + return; + } + } + + if (!entityNamespace.empty()) + { + // TODO: Mpelka - remove this error when ROS 2 namespace is implemented + AZ_Error("SimulationInterfaces", false, "ROS 2 namespace is not implemented yet in spawning"); + completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "This feature is not implemented yet in spawning entities"))); + return; + } + // get rel path from uri const AZStd::string relPath = Utils::UriToRelPath(uri); - // create spawnnable + // create spawnable AZ::Data::AssetId assetId; AZ::Data::AssetCatalogRequestBus::BroadcastResult( assetId, @@ -591,12 +612,6 @@ namespace SimulationInterfaces { transformInterface->SetWorldTM(initialPose); } - - if (!entityNamespace.empty()) - { - // TODO: Mpelka set ROS 2 namespace here - AZ_Error("SimulationInterfaces", false, "ROS 2 namespace is not implemented yet in spawning"); - } }; optionalArgs.m_completionCallback = [this, uri](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index 5bd7a3d26a..e9e01448e4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -48,6 +48,7 @@ namespace SimulationInterfaces const AZStd::string& uri, const AZStd::string& entityNamespace, const AZ::Transform& initialPose, + const bool allowRename, SpawnCompletedCb completedCb) override; // AZ::Component interface implementation diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp index 3527def891..c0771f0def 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -66,13 +66,24 @@ namespace UnitTest completed = true; }; + constexpr bool allowRename = false; SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, completedCb); - + &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, completedCb); // entities are spawned asynchronously, so we need to tick the app to let the entity be spawned TickApp(100); 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( diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp index ca130ae78a..2a3db62152 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -29,13 +29,13 @@ namespace SimulationInterfacesROS2 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() }; 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; diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h index ae32417825..19bc334973 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h @@ -23,9 +23,9 @@ namespace UnitTest MOCK_METHOD2(SetEntityState, AZ::Outcome(const AZStd::string& name, const EntityState& state)); MOCK_METHOD2(DeleteEntity, void(const AZStd::string& name, DeletionCompletedCb completedCb)); MOCK_METHOD0(GetSpawnables, AZ::Outcome()); - MOCK_METHOD5( + MOCK_METHOD6( SpawnEntity, - void(const AZStd::string& name, const AZStd::string& uri, const AZStd::string& entityNamespace, const AZ::Transform& initialPose, SpawnCompletedCb completedCb)); + void(const AZStd::string& name, const AZStd::string& uri, const AZStd::string& entityNamespace, const AZ::Transform& initialPose, const bool allowRename, SpawnCompletedCb completedCb)); }; } From e537b8fad789ddbd2050a825c1e5aa8082fd31b9 Mon Sep 17 00:00:00 2001 From: Patryk Antosz Date: Wed, 9 Apr 2025 11:15:32 +0200 Subject: [PATCH 11/35] SimulationInterfacesROS2 - added action server for SimulateSteps (#861) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added handler for new action - SimulateSteps *added new base class for the actions *extended base interface for all services, actions and topics handlers *fixed some minor bugs *fixed typos *updated gem.json *updated logs msgs and comments --------- Signed-off-by: Patryk Antosz Co-authored-by: Michał Pełka Co-authored-by: Norbert Prokopiuk Co-authored-by: Jan Hanca --- .../SimulationInterfacesTypeIds.h | 1 + .../SimulationMangerRequestBus.h | 36 ++++- .../Code/Source/Clients/SimulationManager.cpp | 29 +++- .../Code/Source/Clients/SimulationManager.h | 9 +- .../Code/CMakeLists.txt | 3 +- .../Code/Source/Actions/ROS2ActionBase.h | 127 ++++++++++++++++++ .../SimulateStepsActionServerHandler.cpp | 121 +++++++++++++++++ .../SimulateStepsActionServerHandler.h | 50 +++++++ ...imulationInterfacesROS2SystemComponent.cpp | 24 ++-- .../SimulationInterfacesROS2SystemComponent.h | 2 +- .../Code/Source/Interfaces/IROS2HandlerBase.h | 2 + .../Services/DeleteEntityServiceHandler.cpp | 1 - .../Services/DeleteEntityServiceHandler.h | 5 +- .../Services/GetEntitiesServiceHandler.cpp | 2 +- .../Services/GetEntitiesServiceHandler.h | 5 +- .../GetEntitiesStatesServiceHandler.cpp | 19 ++- .../GetEntitiesStatesServiceHandler.h | 5 +- .../Services/GetEntityStateServiceHandler.cpp | 1 - .../Services/GetEntityStateServiceHandler.h | 5 +- .../GetSimulationFeaturesServiceHandler.cpp | 3 +- .../GetSimulationFeaturesServiceHandler.h | 9 +- .../Services/GetSpawnablesServiceHandler.h | 5 +- ...S2HandlerBaseClass.h => ROS2ServiceBase.h} | 59 ++++---- .../Services/SetEntityStateServiceHandler.cpp | 2 - .../Services/SetEntityStateServiceHandler.h | 5 +- .../Services/SpawnEntityServiceHandler.cpp | 2 - .../Services/SpawnEntityServiceHandler.h | 5 +- .../Code/Source/Utils/RegistryUtils.cpp | 2 +- .../Code/Source/Utils/RegistryUtils.h | 2 +- .../Code/Source/Utils/Utils.h | 2 +- ...mulationinterfacesros2_private_files.cmake | 7 +- Gems/SimulationInterfacesROS2/gem.json | 58 ++++---- 32 files changed, 488 insertions(+), 120 deletions(-) create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h rename Gems/SimulationInterfacesROS2/Code/Source/Services/{ROS2HandlerBaseClass.h => ROS2ServiceBase.h} (84%) diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h index 9a69035fa1..bea5048d6c 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h @@ -31,5 +31,6 @@ namespace SimulationInterfaces inline constexpr const char* SimulationInterfacesRequestsTypeId = "{6818E5E3-BBF5-41BD-96BB-7AF57CCC7528}"; inline constexpr const char* SimulationManagerRequestsTypeId = "{056477BA-8153-4901-9401-0146A5E3E9ED}"; inline constexpr const char* SimulationFeaturesAggregatorRequestsTypeId = "{099fd08b-b0e2-4705-9c35-cc09c8e45076}"; + inline constexpr const char* SimulationManagerNotificationsTypeId = "{0201067b-9d52-4ab7-9a45-284287f53b00}"; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 05bc8cbc34..bc87d3f3ea 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -30,7 +30,18 @@ namespace SimulationInterfaces //! Step the simulation by a number of steps //! expect always to succeed - virtual void StepSimulation(AZ::u32 steps) = 0; + virtual void StepSimulation(AZ::u64 steps) = 0; + + //! Check whether the simulation is paused or not + //! @return boolean value indicating the pause state of the simulation + virtual bool IsSimulationPaused() const = 0; + + //! Cancel executing the simulation steps, if there are remaining steps left + virtual void CancelStepSimulation() = 0; + + //! Check if the SimulationSteps is active + //! @return boolean value indicating if the SimulationSteps is active + virtual bool IsSimulationStepsActive() const = 0; }; class SimulationMangerRequestBusTraits : public AZ::EBusTraits @@ -46,4 +57,27 @@ namespace SimulationInterfaces using SimulationManagerRequestBus = AZ::EBus; 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/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index 252071dbbc..37ffd39420 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -85,6 +85,27 @@ namespace SimulationInterfaces SimulationManagerRequestBus::Handler::BusDisconnect(); } + bool SimulationManager::IsSimulationPaused() const + { + return m_isSimulationPaused; + } + + bool SimulationManager::IsSimulationStepsActive() const + { + return m_simulationFinishEvent.IsConnected(); + } + + void SimulationManager::CancelStepSimulation() + { + if (m_simulationFinishEvent.IsConnected()) + { + m_simulationFinishEvent.Disconnect(); + SetSimulationPaused(true); + m_numberOfPhysicsSteps = 0; + } + } + + void SimulationManager::SetSimulationPaused(bool paused) { // get az physics system @@ -97,11 +118,16 @@ namespace SimulationInterfaces { AZ_Assert(scene, "Physics scene is not available"); scene->SetEnabled(!paused); + m_isSimulationPaused = paused; } } - void SimulationManager::StepSimulation(AZ::u32 steps) + void SimulationManager::StepSimulation(AZ::u64 steps) { + if (steps == 0) + { + return; + } m_numberOfPhysicsSteps = steps; // install handler @@ -110,6 +136,7 @@ namespace SimulationInterfaces { m_numberOfPhysicsSteps--; AZ_Printf("SimulationManager", "Physics simulation step finished. Remaining steps: %d", m_numberOfPhysicsSteps); + SimulationManagerNotificationsBus::Broadcast(&SimulationManagerNotifications::OnSimulationStepFinish, m_numberOfPhysicsSteps); if (m_numberOfPhysicsSteps <= 0) { SetSimulationPaused(true); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index aa8b5d0ebf..77318a5e06 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -42,8 +42,13 @@ namespace SimulationInterfaces protected: void SetSimulationPaused(bool paused) override; - void StepSimulation(AZ::u32 steps) override; - uint32_t m_numberOfPhysicsSteps = 0; + void StepSimulation(AZ::u64 steps) override; + bool IsSimulationPaused() const override; + void CancelStepSimulation() override; + bool IsSimulationStepsActive() const override; + + bool m_isSimulationPaused = false; + uint64_t m_numberOfPhysicsSteps = 0; AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt index 2179cae4fe..57e0ce3601 100644 --- a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt +++ b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt @@ -30,6 +30,7 @@ ly_add_target( BUILD_DEPENDENCIES INTERFACE AZ::AzCore + Gem::SimulationInterfaces.API ) # The ${gem_name}.Private.Object target is an internal target @@ -55,7 +56,7 @@ ly_add_target( AZ::AzFramework ) -target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces) +target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces rclcpp_action) # Here add ${gem_name} target, it depends on the Private Object library and Public API interface ly_add_target( diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h b/Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h new file mode 100644 index 0000000000..5fbeea2117 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h @@ -0,0 +1,127 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + //! Base for each ROS 2 action server handler, forces declaration of features provided by the server + //! combined information along all ROS 2 handlers gives information about simulation features + //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg + using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures; + template + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp new file mode 100644 index 0000000000..542cbe58fe --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp @@ -0,0 +1,121 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "SimulateStepsActionServerHandler.h" + +namespace SimulationInterfacesROS2 +{ + + void SimulateStepsActionServerHandler::Initialize(rclcpp::Node::SharedPtr& node) + { + ROS2ActionBase::Initialize(node); + SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusConnect(); + } + + SimulateStepsActionServerHandler::~SimulateStepsActionServerHandler() + { + AZ::TickBus::Handler::BusDisconnect(); + SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusDisconnect(); + } + + AZStd::unordered_set SimulateStepsActionServerHandler::GetProvidedFeatures() + { + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_ACTION }; + } + + AZStd::string_view SimulateStepsActionServerHandler::GetTypeName() const + { + return "SimulateSteps"; + } + + AZStd::string_view SimulateStepsActionServerHandler::GetDefaultName() const + { + return "simulate_steps"; + } + + rclcpp_action::GoalResponse SimulateStepsActionServerHandler::GoalReceivedCallback( + [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr 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_isCancelled = 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_isCancelled = false; + AZ::TickBus::Handler::BusConnect(); + SimulationInterfaces::SimulationManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationManagerRequests::StepSimulation, m_goalSteps); + } + + void SimulateStepsActionServerHandler::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + if (m_isCancelled) + { + auto result = std::make_shared(); + result->result.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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h new file mode 100644 index 0000000000..abadc1d57c --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h @@ -0,0 +1,50 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include "ROS2ActionBase.h" +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + + 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_isCancelled{ false }; + }; + +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp index ffb85d213d..7ae1db948a 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -7,14 +7,15 @@ */ #include "SimulationInterfacesROS2SystemComponent.h" -#include "Services/ROS2HandlerBaseClass.h" -#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" + +#include #include +#include +#include #include #include -#include "Utils/RegistryUtils.h" #include #include @@ -27,10 +28,10 @@ namespace SimulationInterfacesROS2 void RegisterInterface( AZStd::unordered_map>& interfacesMap, rclcpp::Node::SharedPtr ros2Node) { - AZStd::shared_ptr service = AZStd::make_shared(); - service->CreateService(ros2Node); - interfacesMap[service->GetTypeName()] = AZStd::move(service); - service.reset(); + AZStd::shared_ptr handler = AZStd::make_shared(); + handler->Initialize(ros2Node); + interfacesMap[handler->GetTypeName()] = AZStd::move(handler); + handler.reset(); }; } // namespace @@ -85,24 +86,25 @@ namespace SimulationInterfacesROS2 RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); } void SimulationInterfacesROS2SystemComponent::Deactivate() { SimulationInterfacesROS2RequestBus::Handler::BusDisconnect(); - for (auto& [serviceType, service] : m_availableRos2Interface) + for (auto& [handlerType, handler] : m_availableRos2Interface) { - service.reset(); + handler.reset(); } } AZStd::unordered_set SimulationInterfacesROS2SystemComponent::GetSimulationFeatures() { AZStd::unordered_set result; - for (auto& [serviceType, serviceHandler] : m_availableRos2Interface) + for (auto& [handlerType, handler] : m_availableRos2Interface) { - auto features = serviceHandler->GetProvidedFeatures(); + auto features = handler->GetProvidedFeatures(); result.insert(features.begin(), features.end()); } return result; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h index 8e773e7985..0d3e7b9e37 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h @@ -17,7 +17,7 @@ #include "Services/GetEntityStateServiceHandler.h" #include "Services/GetSimulationFeaturesServiceHandler.h" #include "Services/GetSpawnablesServiceHandler.h" -#include "Services/ROS2HandlerBaseClass.h" +#include "Services/ROS2ServiceBase.h" #include "Services/SetEntityStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h b/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h index b0d5b6b495..e11a512f81 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h @@ -9,6 +9,7 @@ #pragma once #include #include +#include namespace SimulationInterfacesROS2 { @@ -20,5 +21,6 @@ namespace SimulationInterfacesROS2 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp index 35050db756..2a21efbf58 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -7,7 +7,6 @@ */ #include "DeleteEntityServiceHandler.h" -#include #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h index d9e66c8455..7a48eb243e 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h @@ -8,15 +8,14 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class DeleteEntityServiceHandler : public ROS2HandlerBase + class DeleteEntityServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp index a78ed7d043..9640ca33b7 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -7,9 +7,9 @@ */ #include "GetEntitiesServiceHandler.h" -#include "Utils/Utils.h" #include #include +#include namespace SimulationInterfacesROS2 { diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h index ee322a9cef..c4e96a3430 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h @@ -8,14 +8,13 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class GetEntitiesServiceHandler : public ROS2HandlerBase + class GetEntitiesServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index 02baaabcbd..1afb40a628 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -7,11 +7,10 @@ */ #include "GetEntitiesStatesServiceHandler.h" -#include "Utils/Utils.h" #include #include #include -#include +#include namespace SimulationInterfacesROS2 { @@ -70,15 +69,13 @@ namespace SimulationInterfacesROS2 [](const auto& pair) { const SimulationInterfaces::EntityState& entityState = pair.second; - simulation_interfaces::msg::EntityState simulationInterfaceEntityState; - std_msgs::msg::Header header; - header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); - header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); - simulationInterfaceEntityState.header = header; - simulationInterfaceEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); - simulationInterfaceEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); - simulationInterfaceEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); - return simulationInterfaceEntityState; + simulation_interfaces::msg::EntityState simulationInterfacesEntityState; + simulationInterfacesEntityState.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + simulationInterfacesEntityState.header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); + simulationInterfacesEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); + simulationInterfacesEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); + simulationInterfacesEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); + return simulationInterfacesEntityState; }); response.entities = stdEntities; response.states = stdEntityStates; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h index 88ae42fe40..81e857e816 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h @@ -8,15 +8,14 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class GetEntitiesStatesServiceHandler : public ROS2HandlerBase + class GetEntitiesStatesServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp index 593c0a5a3c..479e5c8fa6 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -7,7 +7,6 @@ */ #include "GetEntityStateServiceHandler.h" -#include #include #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h index 05e3bd6e6b..bc9eb929c2 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h @@ -8,15 +8,14 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class GetEntityStateServiceHandler : public ROS2HandlerBase + class GetEntityStateServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 445987f125..58c81c2fa5 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -7,11 +7,10 @@ */ #include "GetSimulationFeaturesServiceHandler.h" -#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" #include #include -#include #include +#include namespace SimulationInterfacesROS2 { diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h index fd7689e90e..67c53f6bb3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -7,16 +7,15 @@ */ #pragma once -#include "Services/ROS2HandlerBaseClass.h" + +#include "ROS2ServiceBase.h" #include -#include -#include -#include +#include namespace SimulationInterfacesROS2 { - class GetSimulationFeaturesServiceHandler : public ROS2HandlerBase + class GetSimulationFeaturesServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h index 35738f1f80..b17c7b51bb 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h @@ -8,15 +8,14 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class GetSpawnablesServiceHandler : public ROS2HandlerBase + class GetSpawnablesServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h similarity index 84% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h rename to Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h index 5498ac8f06..64c50bbdc3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2HandlerBaseClass.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h @@ -8,31 +8,57 @@ #pragma once -#include "Interfaces/IROS2HandlerBase.h" -#include "Utils/RegistryUtils.h" #include #include +#include +#include #include #include #include + namespace SimulationInterfacesROS2 { - //! base for each ros2 service handler, forces declaration of features provided by the handler - //! combined informations along all ROS 2 handlers gives information about simulation features + //! Base for each ROS 2 service handler, forces declaration of features provided by the handler + //! combined information along all ROS 2 handlers gives information about simulation features //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg using SimulationFeatures = simulation_interfaces::msg::SimulatorFeatures; template - class ROS2HandlerBase : public virtual IROS2HandlerBase + class ROS2ServiceBase : public virtual IROS2HandlerBase { public: using Request = typename RosServiceType::Request; using Response = typename RosServiceType::Response; using ServiceHandle = std::shared_ptr>; - virtual ~ROS2HandlerBase() = default; + virtual ~ROS2ServiceBase() = default; + + void Initialize(rclcpp::Node::SharedPtr& node) override + { + CreateService(node); + } + + void SendResponse(Response response) + { + AZ_Assert(m_serviceHandle, "Failed to get m_serviceHandle"); + AZ_Assert(m_lastRequestHeader, "Failed to get last request header ptr"); + m_serviceHandle->send_response(*m_lastRequestHeader, response); + } + + protected: + //! This function is called when a service request is received. + virtual AZStd::optional 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::GetServiceName(GetTypeName()); + AZStd::string serviceName = RegistryUtilities::GetName(GetTypeName()); if (serviceName.empty()) { @@ -59,25 +85,6 @@ namespace SimulationInterfacesROS2 }); } - void SendResponse(Response response) - { - AZ_Assert(m_serviceHandle, "Failed to get m_serviceHandle"); - AZ_Assert(m_lastRequestHeader, "Failed to get last request header ptr"); - m_serviceHandle->send_response(*m_lastRequestHeader, response); - } - - protected: - //! This function is called when a service request is received. - virtual AZStd::optional 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: std::shared_ptr m_lastRequestHeader = nullptr; ServiceHandle m_serviceHandle; }; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp index 8926a4ce1b..e0b9c17fbe 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -7,7 +7,6 @@ */ #include "SetEntityStateServiceHandler.h" -#include #include #include @@ -39,7 +38,6 @@ namespace SimulationInterfacesROS2 const auto& failedResult = outcome.GetError(); response.result.result = aznumeric_cast(failedResult.error_code); response.result.error_message = failedResult.error_string.c_str(); - return response; } return response; diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h index 92471117ae..f176f574cf 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h @@ -8,15 +8,14 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class SetEntityStateServiceHandler : public ROS2HandlerBase + class SetEntityStateServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp index 2a3db62152..dc89dbe9cd 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -7,8 +7,6 @@ */ #include "SpawnEntityServiceHandler.h" -#include "Services/ROS2HandlerBaseClass.h" -#include #include #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h index 138be818da..c477ae91cb 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h @@ -8,14 +8,13 @@ #pragma once -#include "Services/ROS2HandlerBaseClass.h" +#include "ROS2ServiceBase.h" #include -#include #include namespace SimulationInterfacesROS2 { - class SpawnEntityServiceHandler : public ROS2HandlerBase + class SpawnEntityServiceHandler : public ROS2ServiceBase { public: AZStd::string_view GetTypeName() const override diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp index 4e4f38d384..e72b4481e3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp @@ -12,7 +12,7 @@ namespace SimulationInterfacesROS2::RegistryUtilities { - AZStd::string GetServiceName(AZStd::string serviceType) + AZStd::string GetName(AZStd::string serviceType) { AZ::SettingsRegistryInterface* settingsRegistry = AZ::SettingsRegistry::Get(); AZ_Assert(settingsRegistry, "Settings Registry is not available"); diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h index 18392f00a3..cb2b92b823 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h @@ -16,5 +16,5 @@ namespace SimulationInterfacesROS2::RegistryUtilities //! Gets name of the service with defined type form settings registry //! @return string with service name, if setting registry doesn't exits returns empty string - [[nodiscard]] AZStd::string GetServiceName(AZStd::string serviceType); + [[nodiscard]] AZStd::string GetName(AZStd::string serviceType); } // namespace SimulationInterfacesROS2::RegistryUtilities diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h index e5a8f41905..9b5bc61f1c 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h @@ -42,7 +42,7 @@ namespace SimulationInterfacesROS2::Utils { if (request.filters.bounds.points.size() < 3) { - return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have exactly 2 points."); + return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have at least 3 points."); } filter.m_bounds_shape = AZStd::make_shared(); } diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake index d35354c036..c1fe09d602 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -5,7 +5,10 @@ set(FILES Source/Clients/SimulationInterfacesROS2SystemComponent.cpp Source/Clients/SimulationInterfacesROS2SystemComponent.h Source/Interfaces/IROS2HandlerBase.h - Source/Services/ROS2HandlerBaseClass.h + Source/Actions/SimulateStepsActionServerHandler.cpp + Source/Actions/SimulateStepsActionServerHandler.h + Source/Actions/ROS2ActionBase.h + Source/Services/ROS2ServiceBase.h Source/Services/GetEntitiesServiceHandler.cpp Source/Services/GetEntitiesServiceHandler.h Source/Services/DeleteEntityServiceHandler.cpp @@ -28,5 +31,5 @@ set(FILES Source/Services/GetSimulationFeaturesServiceHandler.h Source/Utils/RegistryUtils.cpp Source/Utils/RegistryUtils.h - Source/Utils/Utils.h + Source/Utils/Utils.h ) diff --git a/Gems/SimulationInterfacesROS2/gem.json b/Gems/SimulationInterfacesROS2/gem.json index 61e19c816f..24745d49f4 100644 --- a/Gems/SimulationInterfacesROS2/gem.json +++ b/Gems/SimulationInterfacesROS2/gem.json @@ -1,28 +1,34 @@ { - "gem_name": "SimulationInterfacesROS2", - "version": "1.0.0", - "display_name": "SimulationInterfacesROS2", - "license": "License used i.e. Apache-2.0 or MIT", - "license_url": "Link to the license web site i.e. https://opensource.org/licenses/Apache-2.0", - "origin": "The name of the originator or creator", - "origin_url": "The website for this Gem", - "type": "Code", - "summary": "A short description of this Gem", - "canonical_tags": [ - "Gem" - ], - "user_tags": [ - "SimulationInterfacesROS2" - ], - "platforms": [ - "" - ], - "icon_path": "preview.png", - "requirements": "Notice of any requirements for this Gem i.e. This requires X other gem", - "documentation_url": "Link to any documentation of your Gem", - "dependencies": [], - "repo_uri": "", - "compatible_engines": [], - "engine_api_dependencies": [], - "restricted": "SimulationInterfacesROS2" + "gem_name": "SimulationInterfacesROS2", + "version": "1.0.0", + "display_name": "SimulationInterfacesROS2", + "license": "Apache-2.0", + "license_url": "https://opensource.org/licenses/Apache-2.0", + "origin": "RobotecAI", + "origin_url": "https://robotec.ai", + "type": "Code", + "summary": "This gem provides ROS 2 interface for SimulationInterfaces gem.", + "canonical_tags": [ + "Gem" + ], + "user_tags": [ + "SimulationInterfacesROS2", + "SimulationInterfaces", + "ROS2", + "ROS 2" + ], + "platforms": [ + "" + ], + "icon_path": "preview.png", + "requirements": "ROS2, SimulationInterfaces", + "documentation_url": "", + "dependencies": [ + "SimulationInterfaces>=1.0.0", + "ROS2>=3.3.0" + ], + "repo_uri": "", + "compatible_engines": [], + "engine_api_dependencies": [], + "restricted": "SimulationInterfacesROS2" } From 1f3d83495a21af379f8734e662fa53fcbce639d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 9 Apr 2025 11:54:22 +0100 Subject: [PATCH 12/35] Simulation interfaces - reset simulator service (#862) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Simulation reset service implementation * Adjust the ROS 2 API to support time reset. --------- Signed-off-by: Michał Pełka Co-authored-by: Norbert Prokopiuk --- .../Code/Include/ROS2/Clock/ITimeSource.h | 7 ++ Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h | 4 + .../Code/Include/ROS2/Clock/ROS2TimeSource.h | 4 + .../Code/Include/ROS2/Clock/RealTimeSource.h | 3 + .../Include/ROS2/Clock/SimulationTimeSource.h | 2 + Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp | 5 + .../ROS2/Code/Source/Clock/ROS2TimeSource.cpp | 6 + .../ROS2/Code/Source/Clock/RealTimeSource.cpp | 5 + .../Source/Clock/SimulationTimeSource.cpp | 7 ++ .../SimulationEntityManagerRequestBus.h | 3 + .../SimulationMangerRequestBus.h | 5 + .../Code/Source/Clients/ConsoleCommands.inl | 28 +++++ .../Clients/SimulationEntitiesManager.cpp | 29 +++++ .../Clients/SimulationEntitiesManager.h | 1 + .../Code/Source/Clients/SimulationManager.cpp | 45 ++++++- .../Code/Source/Clients/SimulationManager.h | 10 ++ .../Tests/Tools/SimulationInterfaceTests.cpp | 15 +++ .../Tests/Tools/SimulationIterfaceAppTest.cpp | 45 ++++++- ...imulationInterfacesROS2SystemComponent.cpp | 1 + .../SimulationInterfacesROS2SystemComponent.h | 1 + .../ResetSimulationServiceHandler.cpp | 116 ++++++++++++++++++ .../Services/ResetSimulationServiceHandler.h | 35 ++++++ .../Tools/Mocks/SimulationEntityManagerMock.h | 1 + ...mulationinterfacesros2_private_files.cmake | 2 + 24 files changed, 370 insertions(+), 10 deletions(-) create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h 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..ea1d1d486a 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h @@ -12,6 +12,8 @@ #include #include #include +#include +#include namespace ROS2 { @@ -32,6 +34,8 @@ namespace ROS2 builtin_interfaces::msg::Time GetROSTimestamp() const; + 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..4e66aaf37c 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h @@ -25,6 +25,10 @@ namespace ROS2 //! Get ROS 2 time as ROS2 message. //! @see ROS2Requests::GetROSTimestamp() for more details. virtual builtin_interfaces::msg::Time GetROSTimestamp() const override; + + + virtual AZ::Outcome 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..a976c9f3fa 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h @@ -25,6 +25,9 @@ namespace ROS2 virtual void Activate() override {}; virtual void Deactivate() override {}; + + virtual AZ::Outcome AdjustTime(const builtin_interfaces::msg::Time& time) override; + //! Get simulation time as ROS 2 message. //! @see ROS2Requests::GetROSTimestamp() for more details. virtual builtin_interfaces::msg::Time GetROSTimestamp() const override; diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h index 0a91b71ece..9006dd4658 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h @@ -30,6 +30,8 @@ namespace ROS2 virtual void Activate() override; virtual void Deactivate() override; + virtual AZ::Outcome 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/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/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index 19a5bbb46c..581dc93956 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -80,6 +80,9 @@ namespace SimulationInterfaces //! @see DeleteEntity.srv virtual void DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) = 0; + //! Remove all previously spawned entity from the simulation. + virtual void DeleteAllEntities(DeletionCompletedCb completedCb) = 0; + //! Get a list of spawnable entities. //! @see GetSpawnables.srv virtual AZ::Outcome GetSpawnables() = 0; diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index bc87d3f3ea..2ebaf793ff 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -21,9 +21,14 @@ namespace SimulationInterfaces class SimulationManagerRequests { public: + AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); virtual ~SimulationManagerRequests() = default; + //! Reload level and removal of all spawned simulation entities. + using ReloadLevelCallback = AZStd::function; + virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0; + //! Set the simulation to paused or unpaused, //! expect always to succeed virtual void SetSimulationPaused(bool paused) = 0; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl index a6faca84ef..620c808163 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl @@ -226,6 +226,31 @@ namespace SimulationInterfacesCommands AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); } + static void simulationinterfaces_ReloadLevel(const AZ::ConsoleCommandContainer& arguments) + { + SimulationManagerRequests::ReloadLevelCallback cb = []() + { + AZ_Printf("SimulationInterfacesConsole", "Reload level completed\n"); + }; + SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::ReloadLevel, cb); + } + + static void simulationinterfaces_DeleteAll(const AZ::ConsoleCommandContainer& arguments) + { + DeletionCompletedCb cb = [](const AZ::Outcome& result) + { + if (result.IsSuccess()) + { + AZ_Printf("SimulationInterfacesConsole", "All spawned entities deleted\n"); + } + else + { + AZ_Printf("SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().error_string.c_str()); + } + }; + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, cb); + } + AZ_CONSOLEFREEFUNC( simulationinterfaces_GetEntities, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the scene."); @@ -242,4 +267,7 @@ namespace SimulationInterfacesCommands AZ_CONSOLEFREEFUNC( simulationinterfaces_GetSpawnables, AZ::ConsoleFunctorFlags::DontReplicate, "Get all spawnable entities in the scene."); AZ_CONSOLEFREEFUNC(simulationinterfaces_Spawn, AZ::ConsoleFunctorFlags::DontReplicate, "Spawn entity."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_ReloadLevel, AZ::ConsoleFunctorFlags::DontReplicate, "Reload level."); + AZ_CONSOLEFREEFUNC(simulationinterfaces_DeleteAll, AZ::ConsoleFunctorFlags::DontReplicate, "Remove all spawned entities."); + } // namespace SimulationInterfacesCommands diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index c75a324e3d..3ccece75d0 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -514,6 +514,35 @@ namespace SimulationInterfaces #endif } + void SimulationEntitiesManager::DeleteAllEntities(DeletionCompletedCb completedCb) + { + if (m_spawnedTickets.empty()) + { + // early return for empty scene + completedCb(AZ::Success()); + return; + } + for (auto m_spawnedTicket : m_spawnedTickets) + { + auto spawner = AZ::Interface::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; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index e9e01448e4..5a4e45db5a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -42,6 +42,7 @@ namespace SimulationInterfaces 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, diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index 37ffd39420..b3850efe94 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -71,13 +72,19 @@ namespace SimulationInterfaces void SimulationManager::Activate() { + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); SimulationManagerRequestBus::Handler::BusConnect(); SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_PAUSE, - SimulationFeatures::STEP_SIMULATION_SINGLE, - SimulationFeatures::STEP_SIMULATION_MULTIPLE, - SimulationFeatures::STEP_SIMULATION_ACTION }); + AZStd::unordered_set{ + SimulationFeatures::SIMULATION_RESET, + SimulationFeatures::SIMULATION_RESET_TIME, + //SimulationFeatures::SIMULATION_RESET_STATE, + SimulationFeatures::SIMULATION_RESET_SPAWNED, + SimulationFeatures::SIMULATION_STATE_PAUSE, + SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE, + SimulationFeatures::STEP_SIMULATION_ACTION}); } void SimulationManager::Deactivate() @@ -160,4 +167,34 @@ namespace SimulationInterfaces SetSimulationPaused(false); } + void SimulationManager::ReloadLevel(SimulationManagerRequests::ReloadLevelCallback completionCallback) + { + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusConnect(); + m_reloadLevelCallback = completionCallback; + + // We need to delete all entities before reloading the level + DeletionCompletedCb deleteAllCompletion = [](const AZ::Outcome& 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; + } + AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); + } + } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index 77318a5e06..86e22e6294 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -16,11 +16,14 @@ #include #include #include +#include + namespace SimulationInterfaces { class SimulationManager : public AZ::Component , protected SimulationManagerRequestBus::Handler + , protected AzFramework::LevelSystemLifecycleNotificationBus::Handler { public: AZ_COMPONENT_DECL(SimulationManager); @@ -41,14 +44,21 @@ namespace SimulationInterfaces void Deactivate() override; protected: + // SimulationManagerRequestBus interface implementation void SetSimulationPaused(bool paused) override; void StepSimulation(AZ::u64 steps) override; bool IsSimulationPaused() const override; void CancelStepSimulation() override; bool IsSimulationStepsActive() const override; + void ReloadLevel(SimulationManagerRequests::ReloadLevelCallback completionCallback) override; + + // LevelSystemLifecycleNotificationBus interface implementation + void OnLoadingComplete( const char* levelName) override; + private: bool m_isSimulationPaused = false; uint64_t m_numberOfPhysicsSteps = 0; AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; + SimulationManagerRequests::ReloadLevelCallback m_reloadLevelCallback; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp index 69f1256de5..84b5096c66 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp @@ -173,6 +173,21 @@ namespace UnitTest DeleteEntity(entityId1); } + TEST_F(SimulationInterfaceTestFixture, TestEntitySpawnedDeletionEmptyScene) + { + using namespace SimulationInterfaces; + bool deletionWasCompleted = false; + DeletionCompletedCb deleteAllCompletion = [&deletionWasCompleted](const AZ::Outcome& 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 diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp index c0771f0def..63ee40a1e1 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -49,6 +49,16 @@ namespace UnitTest AZ_Assert(assetId.IsValid(), "Failed to get asset id for %s", TestSpawnable.c_str()); } + int getNumberOfEntities() + { + using namespace SimulationInterfaces; + AZ::Outcome enitities; + SimulationEntityManagerRequestBus::BroadcastResult( + enitities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + AZ_Assert(enitities.IsSuccess(), "Failed to get entities"); + return enitities.GetValue().size(); + } + TEST_F(SimulationInterfaceTestFixture, SpawnAppTest) { // This is an integration test that runs the test application with the SimulationInterfaces gem enabled. @@ -69,6 +79,7 @@ namespace UnitTest constexpr bool allowRename = false; SimulationEntityManagerRequestBus::Broadcast( &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, completedCb); + // entities are spawned asynchronously, so we need to tick the app to let the entity be spawned TickApp(100); EXPECT_TRUE(completed); @@ -142,12 +153,34 @@ namespace UnitTest &SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, deletionCompletedCb); TickApp(100); - // list simulation entities after deletion, expect no simulation entities - AZ::Outcome entitiesAfterDeletion; - SimulationEntityManagerRequestBus::BroadcastResult( - entitiesAfterDeletion, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - ASSERT_TRUE(entitiesAfterDeletion.IsSuccess()); - EXPECT_EQ(entitiesAfterDeletion.GetValue().size(), 0); + // check if the entity was deleted + EXPECT_EQ(getNumberOfEntities(), 0); + + // spawn 3 entities of entities and despawn all of them + SpawnCompletedCb cb = [&](const AZ::Outcome& result){}; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity1", uri, entityNamespace, initialPose, cb); + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity2", uri, entityNamespace, initialPose, cb); + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity3", uri, entityNamespace, initialPose, cb); + TickApp(100); + EXPECT_EQ(getNumberOfEntities(), 3); + + // delete all entities + bool deletionWasCompleted = false; + DeletionCompletedCb deleteAllCompletion = [&deletionWasCompleted](const AZ::Outcome& result) + { + deletionWasCompleted = true; + EXPECT_TRUE(result.IsSuccess()); + }; + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); + + TickApp(100); + EXPECT_TRUE(deletionWasCompleted); + EXPECT_EQ(getNumberOfEntities(), 0); + } } // namespace UnitTest diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp index 7ae1db948a..a6fa71d02d 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -86,6 +86,7 @@ namespace SimulationInterfacesROS2 RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); } diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h index 0d3e7b9e37..fd5def2323 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h @@ -20,6 +20,7 @@ #include "Services/ROS2ServiceBase.h" #include "Services/SetEntityStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" +#include "Services/ResetSimulationServiceHandler.h" #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp new file mode 100644 index 0000000000..c00a16be58 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -0,0 +1,116 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "ResetSimulationServiceHandler.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + + 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 == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_STATE) + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED; + response.result.error_message = "Not implemented yet"; + return response; + } + + if (request.scope == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_SPAWNED) + { + auto deletionCompletedCb = [this](const AZ::Outcome& outcome) + { + Response response; + if (outcome.IsSuccess()) + { + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + } + else + { + const auto& failedResult = outcome.GetError(); + response.result.result = aznumeric_cast(failedResult.error_code); + response.result.error_message = failedResult.error_string.c_str(); + } + SendResponse(response); + }; + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::DeleteAllEntities, deletionCompletedCb); + return AZStd::nullopt; + } + + if (request.scope == Request::SCOPE_TIME) + { + auto * interface = ROS2::ROS2Interface::Get(); + AZ_Assert(interface, "ROS2Interface is not available"); + auto& clock = interface->GetSimulationClock(); + + builtin_interfaces::msg::Time time; + time.sec = 0; + time.nanosec = 0; + auto results = clock.AdjustTime(time); + + if (results.IsSuccess()) + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + return response; + } + else + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + const auto & errorMessage = results.GetError(); + response.result.error_message = std::string(errorMessage.c_str(), errorMessage.size()); + return response; + } + } + + if (request.scope == Request::SCOPE_ALL || request.scope == Request::SCOPE_DEFAULT) + { + // check if we are in GameLauncher + AZ::ApplicationTypeQuery appType; + AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationBus::Events::QueryApplicationType, appType); + if (appType.IsValid() && appType.IsEditor()) + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED; + response.result.error_message = "Feature not supported in Editor."; + return response; + } + + auto levelReloadCompletion = [this]() + { + Response response; + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; + SendResponse(response); + }; + SimulationInterfaces::SimulationManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationManagerRequests::ReloadLevel, levelReloadCompletion); + + return AZStd::nullopt; + } + return AZStd::nullopt; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h new file mode 100644 index 0000000000..4d681bf757 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h @@ -0,0 +1,35 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include "ROS2ServiceBase.h" +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h index 19bc334973..8d273d6b31 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h @@ -22,6 +22,7 @@ namespace UnitTest 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_METHOD6( SpawnEntity, diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake index c1fe09d602..4097b881a3 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -27,6 +27,8 @@ set(FILES Source/Services/GetSpawnablesServiceHandler.h Source/Services/SpawnEntityServiceHandler.cpp Source/Services/SpawnEntityServiceHandler.h + Source/Services/ResetSimulationServiceHandler.cpp + Source/Services/ResetSimulationServiceHandler.h Source/Services/GetSimulationFeaturesServiceHandler.cpp Source/Services/GetSimulationFeaturesServiceHandler.h Source/Utils/RegistryUtils.cpp From 1b38dfa933cdd78d5928c20b0b4469cfe045ad9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 9 Apr 2025 13:07:41 +0100 Subject: [PATCH 13/35] [Simulation Interfaces] Adjust error code type - ROS2 way, setting frames (#865) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Adjust error code types * Added frame setting * Introduce dependency on ROS 2 gem to Simulation Interface Gem Signed-off-by: Michał Pełka Co-authored-by: Norbert Prokopiuk --- Gems/SimulationInterfaces/Code/CMakeLists.txt | 3 + .../Include/SimulationInterfaces/Result.h | 12 +--- .../Clients/SimulationEntitiesManager.cpp | 58 ++++++++++++++----- Gems/SimulationInterfaces/gem.json | 2 +- .../Code/Tests/Tools/InterfacesTest.cpp | 2 +- 5 files changed, 50 insertions(+), 27 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/CMakeLists.txt b/Gems/SimulationInterfaces/Code/CMakeLists.txt index 47785f66fe..b9d34e7e75 100644 --- a/Gems/SimulationInterfaces/Code/CMakeLists.txt +++ b/Gems/SimulationInterfaces/Code/CMakeLists.txt @@ -55,7 +55,10 @@ ly_add_target( PUBLIC AZ::AzCore AZ::AzFramework + PRIVATE + Gem::ROS2.Static ) +target_depends_on_ros2_packages(${gem_name}.Private.Object simulation_interfaces) # Here add ${gem_name} target, it depends on the Private Object library and Public API interface ly_add_target( diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h index 3d449a1959..d7fdeca067 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h @@ -13,24 +13,18 @@ namespace SimulationInterfaces { //! Result codes to be used in the Result message //! @see Result.msg - enum class ErrorCode - { - RESULT_FEATURE_UNSUPPORTED = 0, - RESULT_NOT_FOUND = 2, - RESULT_INCORRECT_STATE = 3, - RESULT_OPERATION_FAILED = 4 - }; + using ErrorCodeValue = uint8_t; //! A message type to represent the result of a failed operation struct FailedResult { FailedResult() = default; - FailedResult(ErrorCode error_code, const AZStd::string& error_string) + FailedResult(ErrorCodeValue error_code, const AZStd::string& error_string) : error_code(error_code) , error_string(error_string) { } - ErrorCode error_code; + ErrorCodeValue error_code; AZStd::string error_string; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 3ccece75d0..3884f350f8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -30,6 +30,9 @@ #include #include +#include +#include +#include namespace SimulationInterfaces { void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) @@ -277,7 +280,7 @@ namespace SimulationInterfaces if (!filter.m_tags_filter.m_tags.empty()) { AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); - return AZ::Failure(FailedResult(ErrorCode::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); } const bool reFilter = !filter.m_filter.empty(); @@ -304,7 +307,7 @@ namespace SimulationInterfaces if (m_physicsScenesHandle == AzPhysics::InvalidSceneHandle) { - return AZ::Failure(FailedResult(ErrorCode::RESULT_OPERATION_FAILED, "Physics scene interface is not available.")); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, "Physics scene interface is not available.")); } AzPhysics::OverlapRequest request; @@ -331,7 +334,7 @@ namespace SimulationInterfaces if (!regex.Valid()) { AZ_Warning("SimulationInterfaces", false, "Invalid regex filter"); - return AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Invalid regex filter")); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Invalid regex filter")); } AZStd::ranges::copy_if( prefilteredEntities, @@ -350,7 +353,7 @@ namespace SimulationInterfaces if (findIt == m_simulatedEntityToEntityIdMap.end()) { AZ_Warning("SimulationInterfaces", findIt != m_simulatedEntityToEntityIdMap.end(), "Entity %s not found", name.c_str()); - return AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found")); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found")); } EntityState entityState{}; const AZ::EntityId entityId = findIt->second; @@ -376,7 +379,7 @@ namespace SimulationInterfaces if (!filter.m_tags_filter.m_tags.empty()) { AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); - return AZ::Failure(FailedResult(ErrorCode::RESULT_FEATURE_UNSUPPORTED, "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); @@ -402,7 +405,7 @@ namespace SimulationInterfaces if (findIt == m_simulatedEntityToEntityIdMap.end()) { AZ_Warning("SimulationInterfaces", false, "Entity %s not found", name.c_str()); - return AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found")); + return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found")); } const AZ::EntityId entityId = findIt->second; @@ -454,7 +457,7 @@ namespace SimulationInterfaces if (findIt == m_simulatedEntityToEntityIdMap.end()) { - completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found"))); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found"))); return; } @@ -467,7 +470,7 @@ namespace SimulationInterfaces if (entity == nullptr) { AZ_Error("SimulationInterfaces", false, "Entity %s (%s) not found", name.c_str(), entityId.ToString().c_str()); - completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "Entity not found"))); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, "Entity not found"))); return; } // check if entity is spawned by this component @@ -493,7 +496,7 @@ namespace SimulationInterfaces 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(ErrorCode::RESULT_OPERATION_FAILED, msg))); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, msg))); } #ifdef POTENTIALY_UNSAFE if (findIt != m_simulatedEntityToEntityIdMap.end()) @@ -574,22 +577,27 @@ namespace SimulationInterfaces 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(ErrorCode::RESULT_INCORRECT_STATE, msg))); + 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 (!entityNamespace.empty()) + if (!initialPose.IsOrthogonal()) { - // TODO: Mpelka - remove this error when ROS 2 namespace is implemented - AZ_Error("SimulationInterfaces", false, "ROS 2 namespace is not implemented yet in spawning"); - completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, "This feature is not implemented yet in spawning entities"))); + 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; } @@ -614,7 +622,7 @@ namespace SimulationInterfaces if (!spawnableAsset) { const auto msg = AZStd::string::format("Spawnable asset %s not found", uri.c_str()); - completedCb(AZ::Failure(FailedResult(ErrorCode::RESULT_NOT_FOUND, msg))); + completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_NOT_FOUND, msg))); return; } @@ -629,6 +637,24 @@ namespace SimulationInterfaces } const AZ::Entity* root = *view.begin(); + 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())); + } + } + } + + // change names for all entites for (auto* entity : view) { diff --git a/Gems/SimulationInterfaces/gem.json b/Gems/SimulationInterfaces/gem.json index 6f1244c140..f0316347e9 100644 --- a/Gems/SimulationInterfaces/gem.json +++ b/Gems/SimulationInterfaces/gem.json @@ -20,7 +20,7 @@ "icon_path": "preview.png", "requirements": "", "documentation_url": "", - "dependencies": [], + "dependencies": ["ROS2"], "repo_uri": "", "compatible_engines": [], "engine_api_dependencies": [], diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp index 341b5c3ad4..3f74685e90 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp @@ -189,7 +189,7 @@ namespace UnitTest { EXPECT_EQ(name, "test_entity"); FailedResult failedResult; - failedResult.error_code = SimulationInterfaces::ErrorCode::RESULT_NOT_FOUND; + failedResult.error_code = simulation_interfaces::msg::Result::RESULT_NOT_FOUND, failedResult.error_string = "FooBar not found."; completedCb(AZ::Failure(failedResult)); })); From 2f96b6f3cc8358532ae88af8b232786a24ad95be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 9 Apr 2025 13:19:39 +0100 Subject: [PATCH 14/35] Fix compilation error in SimulationIterfaceAppTest (#867) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Code/Tests/Tools/SimulationIterfaceAppTest.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp index 63ee40a1e1..50058b99fe 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -159,11 +159,11 @@ namespace UnitTest // 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, cb); + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity1", uri, entityNamespace, initialPose, false, cb); SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity2", uri, entityNamespace, initialPose, cb); + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity2", uri, entityNamespace, initialPose, false, cb); SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity3", uri, entityNamespace, initialPose, cb); + &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity3", uri, entityNamespace, initialPose, false, cb); TickApp(100); EXPECT_EQ(getNumberOfEntities(), 3); From 353a245fb7b32adf27e82148618db9143e1ca92d Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Wed, 9 Apr 2025 14:42:53 +0200 Subject: [PATCH 15/35] Removed copied simulation_interfaces msgs (#868) Signed-off-by: Norbert Prokopiuk --- .../SimulationInterfaces/SimulationFeatures.h | 47 ------------------- .../SimulationFeaturesAggregatorRequestBus.h | 4 +- .../Clients/SimulationEntitiesManager.cpp | 41 ++++++++-------- .../Code/Source/Clients/SimulationManager.cpp | 23 +++++---- .../Code/simulationinterfaces_api_files.cmake | 1 - .../GetSimulationFeaturesServiceHandler.cpp | 6 +-- .../Code/Tests/Tools/InterfacesTest.cpp | 25 +++++----- 7 files changed, 48 insertions(+), 99 deletions(-) delete mode 100644 Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h deleted file mode 100644 index 4ad2e72e8f..0000000000 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * 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 -{ - - //! Simulation Features copied from the simulation_interfaces - //! to avoid ros2 dependency - //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg - enum class SimulationFeatures : AZ::u8 - { - SPAWNING = 0, - DELETING = 1, - NAMED_POSES = 2, - POSE_BOUNDS = 3, - ENTITY_TAGS = 4, - ENTITY_BOUNDS = 5, - ENTITY_BOUNDS_BOX = 6, - ENTITY_BOUNDS_CONVEX = 7, - ENTITY_CATEGORIES = 8, - SPAWNING_RESOURCE_STRING = 9, - ENTITY_STATE_GETTING = 10, - ENTITY_STATE_SETTING = 11, - ENTITY_INFO_GETTING = 12, - ENTITY_INFO_SETTING = 13, - SPAWNABLES = 14, - SIMULATION_RESET = 20, - SIMULATION_RESET_TIME = 21, - SIMULATION_RESET_STATE = 22, - SIMULATION_RESET_SPAWNED = 23, - SIMULATION_STATE_GETTING = 24, - SIMULATION_STATE_SETTING = 25, - SIMULATION_STATE_PAUSE = 26, - STEP_SIMULATION_SINGLE = 31, - STEP_SIMULATION_MULTIPLE = 32, - STEP_SIMULATION_ACTION = 33 - }; -} // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h index eca8dcec13..b5b325d842 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -10,13 +10,13 @@ #include -#include "SimulationFeatures.h" #include #include #include namespace SimulationInterfaces { + using SimulationFeatures = uint8_t; class SimulationFeaturesAggregatorRequests { @@ -32,7 +32,7 @@ namespace SimulationInterfaces virtual const AZStd::unordered_set GetSimulationFeatures() const = 0; //! Method checks if feature with given id is available in the simulation - //! Method is extenstion to standard defined in simulation_interfaces + //! Method is extension to standard defined in simulation_interfaces virtual bool HasFeature(SimulationFeatures feature) const = 0; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 3884f350f8..2f55ca48e9 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -13,7 +13,6 @@ #include "Clients/SimulationFeaturesAggregator.h" #include "CommonUtilities.h" #include "ConsoleCommands.inl" -#include "SimulationInterfaces/SimulationFeatures.h" #include "SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h" #include #include @@ -30,9 +29,10 @@ #include #include +#include #include +#include #include -#include namespace SimulationInterfaces { void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) @@ -211,15 +211,15 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, - SimulationFeatures::ENTITY_BOUNDS_BOX, - SimulationFeatures::ENTITY_BOUNDS_CONVEX, - SimulationFeatures::ENTITY_CATEGORIES, - SimulationFeatures::ENTITY_STATE_GETTING, - SimulationFeatures::ENTITY_STATE_SETTING, - SimulationFeatures::DELETING, - SimulationFeatures::SPAWNABLES, - SimulationFeatures::SPAWNING }); + AZStd::unordered_set{ simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, + simulation_interfaces::msg::SimulatorFeatures::DELETING, + simulation_interfaces::msg::SimulatorFeatures::SPAWNABLES, + simulation_interfaces::msg::SimulatorFeatures::SPAWNING }); } void SimulationEntitiesManager::Deactivate() @@ -280,7 +280,8 @@ namespace SimulationInterfaces if (!filter.m_tags_filter.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")); + return AZ::Failure( + FailedResult(simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); } const bool reFilter = !filter.m_filter.empty(); @@ -307,7 +308,8 @@ namespace SimulationInterfaces if (m_physicsScenesHandle == AzPhysics::InvalidSceneHandle) { - return AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, "Physics scene interface is not available.")); + return AZ::Failure( + FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, "Physics scene interface is not available.")); } AzPhysics::OverlapRequest request; @@ -379,7 +381,8 @@ namespace SimulationInterfaces if (!filter.m_tags_filter.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")); + return AZ::Failure( + FailedResult(simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); } MultipleEntitiesStates entitiesStates; const auto& entities = GetEntities(filter); @@ -576,8 +579,6 @@ namespace SimulationInterfaces const bool allowRename, SpawnCompletedCb completedCb) { - - if (!allowRename) { // If API user does not allow renaming, check if name is unique @@ -597,7 +598,8 @@ namespace SimulationInterfaces 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 + completedCb(AZ::Failure(FailedResult( + simulation_interfaces::srv::SpawnEntity::Response::INVALID_POSE, "Initial pose is not orthogonal"))); // INVALID_POSE return; } @@ -645,16 +647,15 @@ namespace SimulationInterfaces const AZStd::string f = frameComponent->GetFrameID(); if (f.empty()) { - frameComponent->SetFrameID(name); + frameComponent->SetFrameID(name); } else { - frameComponent->SetFrameID(AZStd::string::format("%s/%s", entityNamespace.c_str(), f.c_str())); + frameComponent->SetFrameID(AZStd::string::format("%s/%s", entityNamespace.c_str(), f.c_str())); } } } - // change names for all entites for (auto* entity : view) { diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index b3850efe94..d60ba5cab0 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -7,7 +7,6 @@ */ #include "SimulationManager.h" -#include "SimulationInterfaces/SimulationFeatures.h" #include #include @@ -15,6 +14,7 @@ #include #include #include +#include namespace SimulationInterfaces { @@ -76,15 +76,14 @@ namespace SimulationInterfaces SimulationManagerRequestBus::Handler::BusConnect(); SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ - SimulationFeatures::SIMULATION_RESET, - SimulationFeatures::SIMULATION_RESET_TIME, - //SimulationFeatures::SIMULATION_RESET_STATE, - SimulationFeatures::SIMULATION_RESET_SPAWNED, - SimulationFeatures::SIMULATION_STATE_PAUSE, - SimulationFeatures::STEP_SIMULATION_SINGLE, - SimulationFeatures::STEP_SIMULATION_MULTIPLE, - SimulationFeatures::STEP_SIMULATION_ACTION}); + 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 }); } void SimulationManager::Deactivate() @@ -112,7 +111,6 @@ namespace SimulationInterfaces } } - void SimulationManager::SetSimulationPaused(bool paused) { // get az physics system @@ -143,7 +141,8 @@ namespace SimulationInterfaces { m_numberOfPhysicsSteps--; AZ_Printf("SimulationManager", "Physics simulation step finished. Remaining steps: %d", m_numberOfPhysicsSteps); - SimulationManagerNotificationsBus::Broadcast(&SimulationManagerNotifications::OnSimulationStepFinish, m_numberOfPhysicsSteps); + SimulationManagerNotificationsBus::Broadcast( + &SimulationManagerNotifications::OnSimulationStepFinish, m_numberOfPhysicsSteps); if (m_numberOfPhysicsSteps <= 0) { SetSimulationPaused(true); diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake index 69599001b1..186160f3d0 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake @@ -11,5 +11,4 @@ set(FILES Include/SimulationInterfaces/SimulationInterfacesTypeIds.h Include/SimulationInterfaces/SimulationMangerRequestBus.h Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h - Include/SimulationInterfaces/SimulationFeatures.h ) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 58c81c2fa5..4084fbefe9 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -35,10 +35,8 @@ namespace SimulationInterfacesROS2 // common features are logical AND between two sets AZStd::unordered_set commonFeatures; commonFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); - for (auto id : o3deInterfaces) - { - commonFeatures.insert(static_cast(id)); - } + commonFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end()); + Response response; for (auto id : commonFeatures) { diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp index 3f74685e90..99899c3938 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp @@ -25,17 +25,18 @@ #include "Clients/SimulationInterfacesROS2SystemComponent.h" #include "Mocks/SimulationEntityManagerMock.h" +#include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h" #include #include #include #include #include #include +#include #include #include #include #include -#include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h" namespace UnitTest { @@ -99,7 +100,7 @@ namespace UnitTest //! 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."; + ASSERT_NE(GetRos2Node(), nullptr) << "ROS2 node is not available."; } //! Perform a smoke test to check if the ROS2 domain is working @@ -153,7 +154,7 @@ namespace UnitTest auto request = std::make_shared(); const char TestEntityName[] = "test_entity"; request->entity = std::string(TestEntityName); - const AZStd::string entityName = AZStd::string(TestEntityName); + const AZStd::string entityName = AZStd::string(TestEntityName); EXPECT_CALL(*mock, DeleteEntity(entityName, _)) .WillOnce(::testing::Invoke( @@ -181,7 +182,7 @@ namespace UnitTest auto request = std::make_shared(); const char TestEntityName[] = "test_entity"; request->entity = std::string(TestEntityName); - const AZStd::string entityName = AZStd::string(TestEntityName); + const AZStd::string entityName = AZStd::string(TestEntityName); EXPECT_CALL(mock, DeleteEntity(entityName, _)) .WillOnce(::testing::Invoke( @@ -228,10 +229,10 @@ namespace UnitTest if (filter.m_bounds_shape) { EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Sphere); - Physics::SphereShapeConfiguration* sphereShape = azdynamic_cast(filter.m_bounds_shape.get()); + Physics::SphereShapeConfiguration* sphereShape = + azdynamic_cast(filter.m_bounds_shape.get()); EXPECT_EQ(sphereShape->m_radius, 99.0f); EXPECT_EQ(sphereShape->m_scale, AZ::Vector3(1.0f)); - } auto loc = filter.m_bounds_pose.GetTranslation(); EXPECT_EQ(loc.GetX(), point1.GetX()); @@ -298,11 +299,11 @@ namespace UnitTest if (filter.m_bounds_shape) { EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Box); - Physics::BoxShapeConfiguration* boxShape = azdynamic_cast(filter.m_bounds_shape.get()); + Physics::BoxShapeConfiguration* boxShape = + azdynamic_cast(filter.m_bounds_shape.get()); EXPECT_EQ(boxShape->m_dimensions.GetX(), dims.GetX()); EXPECT_EQ(boxShape->m_dimensions.GetY(), dims.GetY()); EXPECT_EQ(boxShape->m_dimensions.GetZ(), dims.GetZ()); - } auto loc = filter.m_bounds_pose.GetTranslation(); EXPECT_EQ(loc, AZ::Vector3::CreateZero()); @@ -320,7 +321,7 @@ namespace UnitTest //! Try to call the service with invalid data (first point is greater than second point in box) TEST_F(SimulationInterfaceROS2TestFixture, GetEntitiesWithShapeBoxInvalid) { - using ::testing::_; + using ::testing::_; auto node = GetRos2Node(); // strict mock, since we don't want to call the real implementation in this test @@ -368,8 +369,8 @@ namespace UnitTest SimulationFeaturesAggregatorRequestsMockedHandler mockAggregator; using ::testing::_; auto node = GetRos2Node(); - AZStd::unordered_set features { - SimulationFeatures::SPAWNING, + 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 }; @@ -391,8 +392,6 @@ namespace UnitTest EXPECT_EQ(response->features.features[0], simulation_interfaces::msg::SimulatorFeatures::SPAWNING) << "Feature is not SPAWNING."; } - - } // namespace UnitTest // required to support running integration tests with Qt and PhysX From 6e78d3dd443c9c072eed2e34c3a29cb5345bd687 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Wed, 9 Apr 2025 16:16:31 +0200 Subject: [PATCH 16/35] Nprokopiuk/simulation interfaces set simulation state (#871) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Placeholder for set simulation state service * Adjusted error codes * Set/Get simulation state with placeholders for deleting entities * Added transition to stopped and quitting --------- Signed-off-by: Norbert Prokopiuk Co-authored-by: Michał Pełka --- .../SimulationMangerRequestBus.h | 15 ++- .../Code/Source/Clients/SimulationManager.cpp | 104 ++++++++++++++++++ .../Code/Source/Clients/SimulationManager.h | 21 +++- ...imulationInterfacesROS2SystemComponent.cpp | 2 + .../SimulationInterfacesROS2SystemComponent.h | 4 +- .../GetSimulationStateServiceHandler.cpp | 32 ++++++ .../GetSimulationStateServiceHandler.h | 36 ++++++ .../SetSimulationStateServiceHandler.cpp | 41 +++++++ .../SetSimulationStateServiceHandler.h | 36 ++++++ ...mulationinterfacesros2_private_files.cmake | 4 + 10 files changed, 290 insertions(+), 5 deletions(-) create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.h create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.h diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 2ebaf793ff..63bb03822a 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -14,14 +14,14 @@ #include #include #include +#include namespace SimulationInterfaces { - + using SimulationState = uint8_t; class SimulationManagerRequests { public: - AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); virtual ~SimulationManagerRequests() = default; @@ -47,6 +47,17 @@ namespace SimulationInterfaces //! 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 diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index d60ba5cab0..ebba7189e8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -7,9 +7,11 @@ */ #include "SimulationManager.h" +#include "SimulationInterfaces/SimulationMangerRequestBus.h" #include #include +#include #include #include #include @@ -18,6 +20,19 @@ namespace SimulationInterfaces { + namespace + { + constexpr AZStd::string_view StartInStoppedStateKey = "/SimulationInterfaces/StartInStoppedState"; + + 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; + } + } // namespace AZ_COMPONENT_IMPL(SimulationManager, "SimulationManager", SimulationManagerTypeId); @@ -84,6 +99,20 @@ namespace SimulationInterfaces simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_SINGLE, simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_MULTIPLE, simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_ACTION }); + AZ::SystemTickBus::QueueFunction( + [this]() + { + // 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()) + { + SetSimulationPaused(true); + } + else + { + SetSimulationState(simulation_interfaces::msg::SimulationState::STATE_PLAYING); + } + }); } void SimulationManager::Deactivate() @@ -196,4 +225,79 @@ namespace SimulationInterfaces 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 (IsTransitionForbidden(stateToSet)) + { + return AZ::Failure(FailedResult( + simulation_interfaces::srv::SetSimulationState::Response::INCORRECT_TRANSITION, + AZStd::string::format("Requested transition (%d -> %d) is forbidden", m_simulationState, stateToSet))); + } + + 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::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(); + } + } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index 86e22e6294..5eea4de0b1 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -11,12 +11,15 @@ #include #include #include +#include +#include +#include #include #include #include #include #include -#include +#include namespace SimulationInterfaces { @@ -51,14 +54,28 @@ namespace SimulationInterfaces 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; + void OnLoadingComplete(const char* levelName) override; private: bool m_isSimulationPaused = false; uint64_t m_numberOfPhysicsSteps = 0; AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; SimulationManagerRequests::ReloadLevelCallback m_reloadLevelCallback; + SimulationState m_simulationState{ + simulation_interfaces::msg::SimulationState::STATE_STOPPED + }; // default simulation state based on standard + private: + 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/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp index a6fa71d02d..23ee473c9e 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -88,6 +88,8 @@ namespace SimulationInterfacesROS2 RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); } void SimulationInterfacesROS2SystemComponent::Deactivate() diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h index fd5def2323..d62c9920e7 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h @@ -16,11 +16,13 @@ #include "Services/GetEntitiesStatesServiceHandler.h" #include "Services/GetEntityStateServiceHandler.h" #include "Services/GetSimulationFeaturesServiceHandler.h" +#include "Services/GetSimulationStateServiceHandler.h" #include "Services/GetSpawnablesServiceHandler.h" #include "Services/ROS2ServiceBase.h" #include "Services/SetEntityStateServiceHandler.h" -#include "Services/SpawnEntityServiceHandler.h" #include "Services/ResetSimulationServiceHandler.h" +#include "Services/SetSimulationStateServiceHandler.h" +#include "Services/SpawnEntityServiceHandler.h" #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.cpp new file mode 100644 index 0000000000..6f4319b3a8 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 SimulationInterfacesROS2 +{ + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.h new file mode 100644 index 0000000000..72e3814314 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.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 "Services/ROS2ServiceBase.h" +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp new file mode 100644 index 0000000000..3422e6141a --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -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 + * + */ + +#include "SetSimulationStateServiceHandler.h" +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + 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 = static_cast(transitionResult.GetError().error_code); + response.result.error_message = transitionResult.GetError().error_string.c_str(); + } + return response; + } +} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.h new file mode 100644 index 0000000000..5672c39a43 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.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 "Services/ROS2ServiceBase.h" +#include +#include +#include + +namespace SimulationInterfacesROS2 +{ + + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake index 4097b881a3..e513431e2a 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -31,6 +31,10 @@ set(FILES 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/Utils/RegistryUtils.cpp Source/Utils/RegistryUtils.h Source/Utils/Utils.h From 748bf1d5654e64c48662976225da8e64c4f74d43 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 10 Apr 2025 10:44:55 +0100 Subject: [PATCH 17/35] [Simulation Interfaces] Validate Namespace and name (#866) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added validation of namespace and name * Added positive and negative test cases * Apply suggestions from code review --------- Signed-off-by: Michał Pełka Co-authored-by: Patryk Antosz --- .../Services/SpawnEntityServiceHandler.cpp | 33 ++++++++ .../Services/SpawnEntityServiceHandler.h | 6 +- .../Code/Tests/Tools/InterfacesTest.cpp | 75 +++++++++++++++++++ 3 files changed, 113 insertions(+), 1 deletion(-) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp index dc89dbe9cd..f927e95c80 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -26,6 +26,27 @@ namespace SimulationInterfacesROS2 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 (!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 frame name + if (!entityNamespace.empty() && !ValidateFrameName(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, @@ -53,4 +74,16 @@ namespace SimulationInterfacesROS2 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::ValidateFrameName(const AZStd::string& frameName) + { + const AZStd::regex frameRegex{ R"(^[a-zA-Z0-9_/]+$)" }; // Entity names can only contain alphanumeric characters and underscores and forward slashes + return AZStd::regex_match(frameName, frameRegex); + } + } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h index c477ae91cb..5a8ef14578 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h @@ -11,7 +11,7 @@ #include "ROS2ServiceBase.h" #include #include - +#include namespace SimulationInterfacesROS2 { class SpawnEntityServiceHandler : public ROS2ServiceBase @@ -31,6 +31,10 @@ namespace SimulationInterfacesROS2 AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; private: + bool ValidateEntityName(const AZStd::string& entityName); + bool ValidateFrameName(const AZStd::string& frameName); + + }; } // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp index 99899c3938..c68e38a2a3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp @@ -392,6 +392,81 @@ namespace UnitTest 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")); + }) + ); + SpinAppSome(); + + 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); + SpinAppSome(); + + 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); + SpinAppSome(); + + 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."; + } } // namespace UnitTest // required to support running integration tests with Qt and PhysX From 81d91b6204b2f1ada7bc1b2abb10e0ae92e2cbd0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 10 Apr 2025 10:45:34 +0100 Subject: [PATCH 18/35] [Simulation Interfaces] step simulation service (#873) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Simulation step service --------- Signed-off-by: Michał Pełka Co-authored-by: Patryk Antosz --- ...imulationInterfacesROS2SystemComponent.cpp | 1 + .../SimulationInterfacesROS2SystemComponent.h | 1 + .../Services/StepSimulationServiceHandler.cpp | 56 +++++++++++++++++++ .../Services/StepSimulationServiceHandler.h | 41 ++++++++++++++ .../Code/Tests/Tools/InterfacesTest.cpp | 2 + ...mulationinterfacesros2_private_files.cmake | 2 + 6 files changed, 103 insertions(+) create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.cpp create mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.h diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp index 23ee473c9e..b3b533c3b4 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp @@ -90,6 +90,7 @@ namespace SimulationInterfacesROS2 RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); RegisterInterface(m_availableRos2Interface, ros2Node); + RegisterInterface(m_availableRos2Interface, ros2Node); } void SimulationInterfacesROS2SystemComponent::Deactivate() diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h index d62c9920e7..df86136bd3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h @@ -23,6 +23,7 @@ #include "Services/ResetSimulationServiceHandler.h" #include "Services/SetSimulationStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" +#include "Services/StepSimulationServiceHandler.h" #include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" #include #include diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.cpp new file mode 100644 index 0000000000..afde3ad772 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 SimulationInterfacesROS2 +{ + + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.h b/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.h new file mode 100644 index 0000000000..4bc3dc654c --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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 SimulationInterfacesROS2 +{ + + 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 SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp index c68e38a2a3..5b5ca156b3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp @@ -142,6 +142,8 @@ namespace UnitTest 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 diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake index e513431e2a..0ade664943 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake @@ -23,6 +23,8 @@ set(FILES Source/Services/DeleteEntityServiceHandler.h Source/Services/SetEntityStateServiceHandler.cpp Source/Services/SetEntityStateServiceHandler.h + Source/Services/StepSimulationServiceHandler.cpp + Source/Services/StepSimulationServiceHandler.h Source/Services/GetSpawnablesServiceHandler.cpp Source/Services/GetSpawnablesServiceHandler.h Source/Services/SpawnEntityServiceHandler.cpp From 474c4dce0855bc16de790c530298401bf3b5b260 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 10 Apr 2025 10:45:46 +0100 Subject: [PATCH 19/35] Adjust capabilities (#872) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Code/Source/Clients/SimulationEntitiesManager.cpp | 6 +++--- .../Code/Source/Clients/SimulationManager.cpp | 4 +++- .../Source/Actions/SimulateStepsActionServerHandler.cpp | 4 +++- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 2f55ca48e9..129743335f 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -211,10 +211,10 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, + AZStd::unordered_set{//simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, + //simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, + //simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING, simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, simulation_interfaces::msg::SimulatorFeatures::DELETING, diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index ebba7189e8..c707fd97ee 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -98,7 +98,9 @@ namespace SimulationInterfaces 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::STEP_SIMULATION_ACTION, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_SETTING, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_GETTING}); AZ::SystemTickBus::QueueFunction( [this]() { diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp index 542cbe58fe..9c8e702277 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp @@ -25,7 +25,9 @@ namespace SimulationInterfacesROS2 AZStd::unordered_set SimulateStepsActionServerHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_ACTION }; + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_ACTION, + SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE }; } AZStd::string_view SimulateStepsActionServerHandler::GetTypeName() const From 2ab4ca2c92eb46b2b05bb2d8a99f19ac3a794845 Mon Sep 17 00:00:00 2001 From: Patryk Antosz Date: Thu, 10 Apr 2025 13:52:29 +0200 Subject: [PATCH 20/35] [Simulation Interfaces] Added simple test for the SimulateSteps action (#869) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added simple test for the SimulateSteps action --------- Signed-off-by: Patryk Antosz Co-authored-by: Michał Pełka --- .../Code/Tests/Tools/InterfacesTest.cpp | 112 ++++++++++++++++++ .../Tests/Tools/Mocks/SimulationManagerMock.h | 30 +++++ ...ioninterfacesros2_editor_tests_files.cmake | 1 + 3 files changed, 143 insertions(+) create mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationManagerMock.h diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp index 5b5ca156b3..2027a82761 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp @@ -26,17 +26,20 @@ #include "Clients/SimulationInterfacesROS2SystemComponent.h" #include "Mocks/SimulationEntityManagerMock.h" #include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h" +#include "Mocks/SimulationManagerMock.h" #include #include #include #include #include #include +#include #include #include #include #include #include +#include namespace UnitTest { @@ -469,6 +472,115 @@ namespace UnitTest 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); + SpinAppSome(); + 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); + SpinAppSome(); + 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); + SpinAppSome(); + 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); + SpinAppSome(); + 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); + SpinAppSome(); + 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 cancelFeautre = client->async_cancel_goal(goalHandle); + SpinAppSome(); + ASSERT_TRUE(cancelFeautre.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 diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationManagerMock.h b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationManagerMock.h new file mode 100644 index 0000000000..475178de97 --- /dev/null +++ b/Gems/SimulationInterfacesROS2/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/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake index e0a6a87474..7fcb5d5c53 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake +++ b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake @@ -1,5 +1,6 @@ set(FILES + Tests/Tools/Mocks/SimulationManagerMock.h Tests/Tools/Mocks/SimulationEntityManagerMock.h Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h Tests/Tools/InterfacesTest.cpp From 42765e00e7bfb0dc7cb9d1bada063a0d525d24cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 10 Apr 2025 15:13:43 +0100 Subject: [PATCH 21/35] [Simulation Interfaces] Support for reset state (#875) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Support for reset state * Fix issue with mixing local and global transform --------- Signed-off-by: Michał Pełka --- .../SimulationEntityManagerRequestBus.h | 4 ++ .../Clients/SimulationEntitiesManager.cpp | 62 +++++++++++++++---- .../Clients/SimulationEntitiesManager.h | 9 ++- .../Code/Source/Clients/SimulationManager.cpp | 2 +- .../ResetSimulationServiceHandler.cpp | 7 ++- .../Tools/Mocks/SimulationEntityManagerMock.h | 1 + 6 files changed, 66 insertions(+), 19 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index 581dc93956..c930b70a47 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -96,6 +96,10 @@ namespace SimulationInterfaces 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 diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 129743335f..47de29bf26 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -157,6 +157,16 @@ namespace SimulationInterfaces spawnData->second.m_completedCb(AZ::Success(registeredName)); m_spawnCompletedCallbacks.erase(spawnData); } + + // cache the initial state + EntityState initialState{}; + initialState.m_pose = entity->GetTransform()->GetLocalTM(); + if (rigidBody) + { + initialState.m_twist_linear = rigidBody->GetLinearVelocity(); + initialState.m_twist_angular = rigidBody->GetAngularVelocity(); + } + m_entityIdToInitialState[entityId] = initialState; }); m_simulationBodyRemovedHandler = AzPhysics::SceneEvents::OnSimulationBodyRemoved::Handler( [this](AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) @@ -273,6 +283,11 @@ namespace SimulationInterfaces m_entityIdToSimulatedEntityMap.erase(findIt); m_simulatedEntityToEntityIdMap.erase(simulatedEntityName); } + auto findIt2 = m_entityIdToInitialState.find(entityId); + if (findIt2 != m_entityIdToInitialState.end()) + { + m_entityIdToInitialState.erase(findIt2); + } } AZ::Outcome SimulationEntitiesManager::GetEntities(const EntityFilters& filter) @@ -417,17 +432,23 @@ namespace SimulationInterfaces // 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) + { + AZ_Assert(!entityAndDescendants.empty(), "Entity and descendants list is empty"); + if (entityAndDescendants.empty()) + { + return; + } + const AZ::EntityId entityId = entityAndDescendants.front(); if (state.m_pose.IsOrthogonal()) { // disable simulation for all entities - AZStd::map entityTransforms; for (const auto& descendant : entityAndDescendants) { - // get name - AZStd::string entityName = "Unknown"; - AZ::ComponentApplicationBus::BroadcastResult(entityName, &AZ::ComponentApplicationRequests::GetEntityName, descendant); - AZ_Printf("SimulationInterfaces", "Disable physics for entity %s\n", entityName.c_str()); Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics); } @@ -451,7 +472,6 @@ namespace SimulationInterfaces SetRigidBodyVelocities(rigidBody, state); } } - return AZ::Success(); } void SimulationEntitiesManager::DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) @@ -480,9 +500,7 @@ namespace SimulationInterfaces const auto ticketId = entity->GetEntitySpawnTicketId(); if (m_spawnedTickets.find(ticketId) != m_spawnedTickets.end()) { - // remove the ticket - // m_spawnedTickets.erase(ticketId); - /// get spawner + // get spawner auto spawner = AZ::Interface::Get(); AZ_Assert(spawner, "SpawnableEntitiesDefinition is not available."); // get ticket @@ -637,7 +655,6 @@ namespace SimulationInterfaces { return; } - const AZ::Entity* root = *view.begin(); for (auto* entity : view) { @@ -663,11 +680,19 @@ namespace SimulationInterfaces entity->SetName(entityName); } - auto* transformInterface = root->FindComponent(); - if (transformInterface) + // get the first entity + if (view.size()>1) { - transformInterface->SetWorldTM(initialPose); + const AZ::Entity* firstEntity = view[1]; + AZ_Assert(firstEntity, "First entity is not available"); + auto* transformInterface = firstEntity->FindComponent(); + if (transformInterface) + { + transformInterface->SetWorldTM(initialPose); + } } + + }; optionalArgs.m_completionCallback = [this, uri](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) @@ -725,4 +750,15 @@ namespace SimulationInterfaces } return simulatedEntityName; } + + 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); + } + } } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index 5a4e45db5a..d32d02e015 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -51,6 +51,7 @@ namespace SimulationInterfaces const AZ::Transform& initialPose, const bool allowRename, SpawnCompletedCb completedCb) override; + void ResetAllEntitiesToInitialState() override; // AZ::Component interface implementation void Init() override; @@ -58,6 +59,8 @@ namespace SimulationInterfaces void Deactivate() override; private: + + //! Registers simulated entity to entity id mapping. //! Note that the entityId will be registered under unique name. //! \param entityId The entity id to register @@ -71,6 +74,9 @@ namespace SimulationInterfaces //! 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; @@ -79,8 +85,7 @@ namespace SimulationInterfaces AzPhysics::SceneHandle m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; AZStd::unordered_map m_simulatedEntityToEntityIdMap; AZStd::unordered_map m_entityIdToSimulatedEntityMap; - AZStd::unordered_set m_disabledBodies; - + AZStd::unordered_map m_entityIdToInitialState; AZStd::unordered_map m_spawnedTickets; struct SpawnCompletedCbData diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index c707fd97ee..b6a60a7029 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -93,7 +93,7 @@ namespace SimulationInterfaces &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_STATE, simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_SPAWNED, simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_PAUSE, simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_SINGLE, diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp index c00a16be58..9b69a0746d 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -23,7 +23,7 @@ namespace SimulationInterfacesROS2 { return AZStd::unordered_set{ SimulationFeatures::SIMULATION_RESET, SimulationFeatures::SIMULATION_RESET_TIME, - //SimulationFeatures::SIMULATION_RESET_STATE, + SimulationFeatures::SIMULATION_RESET_STATE, SimulationFeatures::SIMULATION_RESET_SPAWNED}; } @@ -33,8 +33,9 @@ namespace SimulationInterfacesROS2 if (request.scope == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_STATE) { Response response; - response.result.result = simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED; - response.result.error_message = "Not implemented yet"; + SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( + &SimulationInterfaces::SimulationEntityManagerRequests::ResetAllEntitiesToInitialState); + response.result.result = simulation_interfaces::msg::Result::RESULT_OK; return response; } diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h index 8d273d6b31..11bf4264bd 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h +++ b/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h @@ -24,6 +24,7 @@ namespace UnitTest 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)); From 36b835b5aa4ca6bf46784e9da202600a3be50052 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Thu, 10 Apr 2025 17:12:42 +0200 Subject: [PATCH 22/35] Simulation Interfaces Gems merge (#877) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * SimulationInterfaces and SimulationInterfacesROS2 into one gem Signed-off-by: Norbert Prokopiuk * Namespace changes from SimulationInterfacesROS2 > ROS2SimulationInterfaces, fixed test building Signed-off-by: Norbert Prokopiuk * Changes to make successfull release build with export-project Signed-off-by: Norbert Prokopiuk * rebase missed change added Signed-off-by: Norbert Prokopiuk * Disable empty client tests Signed-off-by: Michał Pełka --------- Signed-off-by: Norbert Prokopiuk Signed-off-by: Michał Pełka Co-authored-by: Michał Pełka --- Gems/SimulationInterfaces/Code/CMakeLists.txt | 35 ++- .../ROS2SimulationInterfacesRequestBus.h} | 20 +- .../ROS2SimulationInterfacesTypeIds.h} | 16 +- .../Code/Platform/Linux/PAL_linux.cmake | 2 +- .../Code/Source/Actions/ROS2ActionBase.h | 4 +- .../SimulateStepsActionServerHandler.cpp | 4 +- .../SimulateStepsActionServerHandler.h | 4 +- ...S2SimulationInterfacesSystemComponent.cpp} | 40 +-- ...ROS2SimulationInterfacesSystemComponent.h} | 18 +- .../Clients/SimulationEntitiesManager.h | 1 - .../Clients/SimulationInterfacesModule.cpp | 4 - .../Code/Source/Clients/SimulationManager.cpp | 6 +- .../Code/Source/Interfaces/IROS2HandlerBase.h | 4 +- .../Services/DeleteEntityServiceHandler.cpp | 4 +- .../Services/DeleteEntityServiceHandler.h | 4 +- .../Services/GetEntitiesServiceHandler.cpp | 4 +- .../Services/GetEntitiesServiceHandler.h | 4 +- .../GetEntitiesStatesServiceHandler.cpp | 4 +- .../GetEntitiesStatesServiceHandler.h | 4 +- .../Services/GetEntityStateServiceHandler.cpp | 4 +- .../Services/GetEntityStateServiceHandler.h | 4 +- .../GetSimulationFeaturesServiceHandler.cpp | 10 +- .../GetSimulationFeaturesServiceHandler.h | 4 +- .../GetSimulationStateServiceHandler.cpp | 4 +- .../GetSimulationStateServiceHandler.h | 4 +- .../Services/GetSpawnablesServiceHandler.cpp | 4 +- .../Services/GetSpawnablesServiceHandler.h | 4 +- .../Code/Source/Services/ROS2ServiceBase.h | 4 +- .../ResetSimulationServiceHandler.cpp | 4 +- .../Services/ResetSimulationServiceHandler.h | 4 +- .../Services/SetEntityStateServiceHandler.cpp | 4 +- .../Services/SetEntityStateServiceHandler.h | 4 +- .../SetSimulationStateServiceHandler.cpp | 4 +- .../SetSimulationStateServiceHandler.h | 4 +- .../Services/SpawnEntityServiceHandler.cpp | 4 +- .../Services/SpawnEntityServiceHandler.h | 4 +- .../Services/StepSimulationServiceHandler.cpp | 4 +- .../Services/StepSimulationServiceHandler.h | 4 +- .../SimulationInterfacesModuleInterface.cpp | 3 + ...lationInterfacesEditorSystemComponent.cpp} | 42 +-- ...mulationInterfacesEditorSystemComponent.h} | 20 +- .../SimulationInterfacesEditorModule.cpp | 7 +- .../Code/Source/Utils/RegistryUtils.cpp | 4 +- .../Code/Source/Utils/RegistryUtils.h | 6 +- .../Code/Source/Utils/Utils.h | 4 +- .../Clients/ROS2SimulationInterfacesTest.cpp | 11 + .../Code/Tests/Tools/InterfacesTest.cpp | 8 +- .../Tools/Mocks/SimulationEntityManagerMock.h | 0 ...ionFeaturesAggregatorRequestsHandlerMock.h | 0 .../Tests/Tools/Mocks/SimulationManagerMock.h | 0 ...s2_simulationinterfaces_tests_files.cmake} | 5 + .../Code/simulationinterfaces_api_files.cmake | 1 + ...ationinterfaces_editor_private_files.cmake | 2 + .../simulationinterfaces_private_files.cmake | 38 +++ Gems/SimulationInterfacesROS2/.clang-format | 61 ---- Gems/SimulationInterfacesROS2/.gitignore | 0 Gems/SimulationInterfacesROS2/CMakeLists.txt | 6 - .../Code/CMakeLists.txt | 262 ------------------ .../Code/Platform/Android/PAL_android.cmake | 4 - .../simulationinterfacesros2_api_files.cmake | 3 - ...mulationinterfacesros2_private_files.cmake | 8 - ...imulationinterfacesros2_shared_files.cmake | 8 - .../Code/Platform/Linux/PAL_linux.cmake | 4 - .../simulationinterfacesros2_api_files.cmake | 3 - ...ationinterfacesros2_editor_api_files.cmake | 3 - ...mulationinterfacesros2_private_files.cmake | 8 - ...imulationinterfacesros2_shared_files.cmake | 8 - .../Code/Platform/Mac/PAL_mac.cmake | 4 - .../simulationinterfacesros2_api_files.cmake | 3 - ...ationinterfacesros2_editor_api_files.cmake | 3 - ...mulationinterfacesros2_private_files.cmake | 8 - ...imulationinterfacesros2_shared_files.cmake | 8 - .../Code/Platform/Windows/PAL_windows.cmake | 4 - .../simulationinterfacesros2_api_files.cmake | 3 - ...ationinterfacesros2_editor_api_files.cmake | 3 - ...mulationinterfacesros2_private_files.cmake | 8 - ...imulationinterfacesros2_shared_files.cmake | 8 - .../Code/Platform/iOS/PAL_ios.cmake | 4 - .../simulationinterfacesros2_api_files.cmake | 3 - ...mulationinterfacesros2_private_files.cmake | 8 - ...imulationinterfacesros2_shared_files.cmake | 8 - .../SimulationInterfacesROS2Module.cpp | 23 -- ...imulationInterfacesROS2ModuleInterface.cpp | 38 --- .../SimulationInterfacesROS2ModuleInterface.h | 27 -- .../SimulationInterfacesROS2EditorModule.cpp | 39 --- .../Clients/SimulationInterfacesROS2Test.cpp | 4 - .../simulationinterfacesros2_api_files.cmake | 4 - ...ationinterfacesros2_editor_api_files.cmake | 4 - ...ninterfacesros2_editor_private_files.cmake | 5 - ...oninterfacesros2_editor_shared_files.cmake | 4 - ...mulationinterfacesros2_private_files.cmake | 43 --- ...imulationinterfacesros2_shared_files.cmake | 4 - ...simulationinterfacesros2_tests_files.cmake | 4 - .../Registry/assetprocessor_settings.setreg | 18 -- Gems/SimulationInterfacesROS2/gem.json | 34 --- Gems/SimulationInterfacesROS2/preview.png | 3 - 96 files changed, 251 insertions(+), 871 deletions(-) rename Gems/{SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h => SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h} (59%) rename Gems/{SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h => SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h} (58%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Actions/ROS2ActionBase.h (98%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Actions/SimulateStepsActionServerHandler.cpp (98%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Actions/SimulateStepsActionServerHandler.h (96%) rename Gems/{SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp => SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp} (72%) rename Gems/{SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h => SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h} (80%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Interfaces/IROS2HandlerBase.h (91%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/DeleteEntityServiceHandler.cpp (95%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/DeleteEntityServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetEntitiesServiceHandler.cpp (97%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetEntitiesServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp (98%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetEntitiesStatesServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetEntityStateServiceHandler.cpp (96%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetEntityStateServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp (84%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetSimulationFeaturesServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetSimulationStateServiceHandler.cpp (94%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetSimulationStateServiceHandler.h (93%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetSpawnablesServiceHandler.cpp (96%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/GetSpawnablesServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/ROS2ServiceBase.h (98%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/ResetSimulationServiceHandler.cpp (98%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/ResetSimulationServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/SetEntityStateServiceHandler.cpp (96%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/SetEntityStateServiceHandler.h (92%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/SetSimulationStateServiceHandler.cpp (95%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/SetSimulationStateServiceHandler.h (93%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/SpawnEntityServiceHandler.cpp (98%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/SpawnEntityServiceHandler.h (93%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/StepSimulationServiceHandler.cpp (96%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Services/StepSimulationServiceHandler.h (93%) rename Gems/{SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp => SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp} (56%) rename Gems/{SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h => SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h} (68%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Utils/RegistryUtils.cpp (87%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Utils/RegistryUtils.h (75%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Source/Utils/Utils.h (97%) create mode 100644 Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Tests/Tools/InterfacesTest.cpp (99%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h (100%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h (100%) rename Gems/{SimulationInterfacesROS2 => SimulationInterfaces}/Code/Tests/Tools/Mocks/SimulationManagerMock.h (100%) rename Gems/{SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake => SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake} (51%) delete mode 100644 Gems/SimulationInterfacesROS2/.clang-format delete mode 100644 Gems/SimulationInterfacesROS2/.gitignore delete mode 100644 Gems/SimulationInterfacesROS2/CMakeLists.txt delete mode 100644 Gems/SimulationInterfacesROS2/Code/CMakeLists.txt delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp delete mode 100644 Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp delete mode 100644 Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h delete mode 100644 Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp delete mode 100644 Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake delete mode 100644 Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg delete mode 100644 Gems/SimulationInterfacesROS2/gem.json delete mode 100644 Gems/SimulationInterfacesROS2/preview.png diff --git a/Gems/SimulationInterfaces/Code/CMakeLists.txt b/Gems/SimulationInterfaces/Code/CMakeLists.txt index b9d34e7e75..e243943ba0 100644 --- a/Gems/SimulationInterfaces/Code/CMakeLists.txt +++ b/Gems/SimulationInterfaces/Code/CMakeLists.txt @@ -58,7 +58,7 @@ ly_add_target( PRIVATE Gem::ROS2.Static ) -target_depends_on_ros2_packages(${gem_name}.Private.Object simulation_interfaces) +target_depends_on_ros2_packages(${gem_name}.Private.Object rclcpp std_msgs geometry_msgs simulation_interfaces rclcpp_action) # Here add ${gem_name} target, it depends on the Private Object library and Public API interface ly_add_target( @@ -140,8 +140,10 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS) Include Source BUILD_DEPENDENCIES + PUBLIC AZ::AzToolsFramework + Gem::ROS2.API ${gem_name}.Private.Object ) @@ -285,6 +287,37 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) 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/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h similarity index 59% rename from Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h rename to Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h index f5fa448ff2..865ec4ded4 100644 --- a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h @@ -8,27 +8,27 @@ #pragma once -#include +#include #include #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { - class SimulationInterfacesROS2Requests + class ROS2SimulationInterfacesRequests { public: - AZ_RTTI(SimulationInterfacesROS2Requests, SimulationInterfacesROS2RequestBusTypeId); - virtual ~SimulationInterfacesROS2Requests() = default; + AZ_RTTI(ROS2SimulationInterfacesRequests, ROS2SimulationInterfacesRequestBusTypeId); + virtual ~ROS2SimulationInterfacesRequests() = default; - //! Returns set of simulation features available in SimulationInterfacesROS2 Gem + //! Returns set of simulation features available in ROS2SimulationInterfaces Gem //! SimulationFeatures follows definition available at: //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg virtual AZStd::unordered_set GetSimulationFeatures() = 0; }; - class SimulationInterfacesROS2RequestBusTraits : public AZ::EBusTraits + class ROS2SimulationInterfacesRequestBusTraits : public AZ::EBusTraits { public: ////////////////////////////////////////////////////////////////////////// @@ -38,7 +38,7 @@ namespace SimulationInterfacesROS2 ////////////////////////////////////////////////////////////////////////// }; - using SimulationInterfacesROS2RequestBus = AZ::EBus; - using SimulationInterfacesROS2RequestBusInterface = AZ::Interface; + using ROS2SimulationInterfacesRequestBus = AZ::EBus; + using ROS2SimulationInterfacesRequestBusInterface = AZ::Interface; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h similarity index 58% rename from Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h rename to Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h index 73246b9519..34d94abd81 100644 --- a/Gems/SimulationInterfacesROS2/Code/Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h @@ -8,19 +8,19 @@ #pragma once -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { // System Component TypeIds - inline constexpr const char* SimulationInterfacesROS2SystemComponentTypeId = "{9CD6E9FA-5C17-454C-B8FA-033DF572B160}"; - inline constexpr const char* SimulationInterfacesROS2EditorSystemComponentTypeId = "{AF5BE964-4B5F-49A4-A308-0B6077E5BB26}"; + 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* SimulationInterfacesROS2ModuleInterfaceTypeId = "{2F1ED7E1-6808-420D-939F-7D5C9CBFB3C9}"; - inline constexpr const char* SimulationInterfacesROS2ModuleTypeId = "{4002B625-F939-44AC-845B-820B20AFC6C5}"; + 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* SimulationInterfacesROS2EditorModuleTypeId = SimulationInterfacesROS2ModuleTypeId; + inline constexpr const char* ROS2SimulationInterfacesEditorModuleTypeId = ROS2SimulationInterfacesModuleTypeId; // API TypeIds - inline constexpr const char* SimulationInterfacesROS2RequestBusTypeId = "{00d08870-e329-4bd7-bb8c-f67fe369de92}"; -} // namespace SimulationInterfacesROS2 + inline constexpr const char* ROS2SimulationInterfacesRequestBusTypeId = "{00d08870-e329-4bd7-bb8c-f67fe369de92}"; +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake b/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake index c79341aef2..238d1b7b24 100644 --- a/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake +++ b/Gems/SimulationInterfaces/Code/Platform/Linux/PAL_linux.cmake @@ -5,5 +5,5 @@ # set(PAL_TRAIT_SIMULATIONINTERFACES_SUPPORTED TRUE) -set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED TRUE) +set(PAL_TRAIT_SIMULATIONINTERFACES_TEST_SUPPORTED FALSE) set(PAL_TRAIT_SIMULATIONINTERFACES_EDITOR_TEST_SUPPORTED TRUE) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h similarity index 98% rename from Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h rename to Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h index 5fbeea2117..6fcacb10bf 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Actions/ROS2ActionBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h @@ -15,7 +15,7 @@ #include #include -namespace SimulationInterfacesROS2 +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 @@ -124,4 +124,4 @@ namespace SimulationInterfacesROS2 ActionHandle m_actionHandle; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp similarity index 98% rename from Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp index 9c8e702277..6fc01cba67 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp @@ -8,7 +8,7 @@ #include "SimulateStepsActionServerHandler.h" -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { void SimulateStepsActionServerHandler::Initialize(rclcpp::Node::SharedPtr& node) @@ -120,4 +120,4 @@ namespace SimulationInterfacesROS2 PublishFeedback(feedback); } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h similarity index 96% rename from Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h rename to Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h index abadc1d57c..1957444487 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Actions/SimulateStepsActionServerHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h @@ -13,7 +13,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class SimulateStepsActionServerHandler @@ -47,4 +47,4 @@ namespace SimulationInterfacesROS2 bool m_isCancelled{ false }; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp similarity index 72% rename from Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp rename to Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp index b3b533c3b4..f3256180ef 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp @@ -6,20 +6,20 @@ * */ -#include "SimulationInterfacesROS2SystemComponent.h" +#include "ROS2SimulationInterfacesSystemComponent.h" #include #include #include -#include +#include #include -#include +#include #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { namespace @@ -36,44 +36,44 @@ namespace SimulationInterfacesROS2 } // namespace AZ_COMPONENT_IMPL( - SimulationInterfacesROS2SystemComponent, "SimulationInterfacesROS2SystemComponent", SimulationInterfacesROS2SystemComponentTypeId); + ROS2SimulationInterfacesSystemComponent, "ROS2SimulationInterfacesSystemComponent", ROS2SimulationInterfacesSystemComponentTypeId); - void SimulationInterfacesROS2SystemComponent::Reflect(AZ::ReflectContext* context) + void ROS2SimulationInterfacesSystemComponent::Reflect(AZ::ReflectContext* context) { if (auto serializeContext = azrtti_cast(context)) { - serializeContext->Class()->Version(0); + serializeContext->Class()->Version(0); } } - void SimulationInterfacesROS2SystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + void ROS2SimulationInterfacesSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) { - provided.push_back(AZ_CRC_CE("SimulationInterfacesROS2Service")); + provided.push_back(AZ_CRC_CE("ROS2SimulationInterfacesService")); } - void SimulationInterfacesROS2SystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + void ROS2SimulationInterfacesSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) { - incompatible.push_back(AZ_CRC_CE("SimulationInterfacesROS2Service")); + incompatible.push_back(AZ_CRC_CE("ROS2SimulationInterfacesService")); } - void SimulationInterfacesROS2SystemComponent::GetRequiredServices( + void ROS2SimulationInterfacesSystemComponent::GetRequiredServices( [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { required.push_back(AZ_CRC_CE("ROS2Service")); } - void SimulationInterfacesROS2SystemComponent::GetDependentServices( + void ROS2SimulationInterfacesSystemComponent::GetDependentServices( [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) { } - void SimulationInterfacesROS2SystemComponent::Init() + void ROS2SimulationInterfacesSystemComponent::Init() { } - void SimulationInterfacesROS2SystemComponent::Activate() + void ROS2SimulationInterfacesSystemComponent::Activate() { - SimulationInterfacesROS2RequestBus::Handler::BusConnect(); + ROS2SimulationInterfacesRequestBus::Handler::BusConnect(); rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); AZ_Assert(ros2Node, "ROS2 node is not available."); @@ -93,9 +93,9 @@ namespace SimulationInterfacesROS2 RegisterInterface(m_availableRos2Interface, ros2Node); } - void SimulationInterfacesROS2SystemComponent::Deactivate() + void ROS2SimulationInterfacesSystemComponent::Deactivate() { - SimulationInterfacesROS2RequestBus::Handler::BusDisconnect(); + ROS2SimulationInterfacesRequestBus::Handler::BusDisconnect(); for (auto& [handlerType, handler] : m_availableRos2Interface) { @@ -103,7 +103,7 @@ namespace SimulationInterfacesROS2 } } - AZStd::unordered_set SimulationInterfacesROS2SystemComponent::GetSimulationFeatures() + AZStd::unordered_set ROS2SimulationInterfacesSystemComponent::GetSimulationFeatures() { AZStd::unordered_set result; for (auto& [handlerType, handler] : m_availableRos2Interface) @@ -114,4 +114,4 @@ namespace SimulationInterfacesROS2 return result; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h similarity index 80% rename from Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h rename to Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h index df86136bd3..64dae28a6c 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2SystemComponent.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h @@ -24,20 +24,20 @@ #include "Services/SetSimulationStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" #include "Services/StepSimulationServiceHandler.h" -#include "SimulationInterfacesROS2/SimulationInterfacesROS2RequestBus.h" +#include "SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h" #include #include #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { - class SimulationInterfacesROS2SystemComponent + class ROS2SimulationInterfacesSystemComponent : public AZ::Component - , public SimulationInterfacesROS2RequestBus::Handler + , public ROS2SimulationInterfacesRequestBus::Handler { public: - AZ_COMPONENT_DECL(SimulationInterfacesROS2SystemComponent); + AZ_COMPONENT_DECL(ROS2SimulationInterfacesSystemComponent); static void Reflect(AZ::ReflectContext* context); @@ -46,8 +46,8 @@ namespace SimulationInterfacesROS2 static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); - SimulationInterfacesROS2SystemComponent() = default; - ~SimulationInterfacesROS2SystemComponent() = default; + ROS2SimulationInterfacesSystemComponent() = default; + ~ROS2SimulationInterfacesSystemComponent() = default; protected: // AZ::Component interface implementation @@ -55,11 +55,11 @@ namespace SimulationInterfacesROS2 void Activate() override; void Deactivate() override; - // SimulationInterfacesROS2RequestBus override + // ROS2SimulationInterfacesRequestBus override AZStd::unordered_set GetSimulationFeatures() override; private: AZStd::unordered_map> m_availableRos2Interface; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index d32d02e015..3146aaa01f 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -60,7 +60,6 @@ namespace SimulationInterfaces private: - //! Registers simulated entity to entity id mapping. //! Note that the entityId will be registered under unique name. //! \param entityId The entity id to register diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp index 119dec747c..a1dc3ba89e 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationInterfacesModule.cpp @@ -20,8 +20,4 @@ namespace SimulationInterfaces }; } // namespace SimulationInterfaces -#if defined(O3DE_GEM_NAME) -AZ_DECLARE_MODULE_CLASS(AZ_JOIN(Gem_, O3DE_GEM_NAME), SimulationInterfaces::SimulationInterfacesModule) -#else AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfaces, SimulationInterfaces::SimulationInterfacesModule) -#endif diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index b6a60a7029..9d8e52d589 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -100,7 +100,7 @@ namespace SimulationInterfaces 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}); + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_GETTING }); AZ::SystemTickBus::QueueFunction( [this]() { @@ -148,7 +148,7 @@ namespace SimulationInterfaces auto* physicsSystem = AZ::Interface::Get(); AZ_Assert(physicsSystem, "Physics system is not available"); const auto& sceneHandlers = physicsSystem->GetAllScenes(); - auto* sceneInterface = AZ::Interface::Get(); + [[maybe_unused]] auto* sceneInterface = AZ::Interface::Get(); AZ_Assert(sceneInterface, "Physics scene interface is not available"); for (auto& scene : sceneHandlers) { @@ -183,7 +183,7 @@ namespace SimulationInterfaces }); // get default scene - auto* physicsSystem = AZ::Interface::Get(); + [[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"); diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h b/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h similarity index 91% rename from Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h rename to Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h index e11a512f81..51599db057 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Interfaces/IROS2HandlerBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h @@ -11,7 +11,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { // common interface to store all simulation feature ros2 handlers in common container class IROS2HandlerBase @@ -23,4 +23,4 @@ namespace SimulationInterfacesROS2 virtual AZStd::string_view GetDefaultName() const = 0; virtual void Initialize(rclcpp::Node::SharedPtr& node) = 0; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp similarity index 95% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp index 2a21efbf58..22812e11ca 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -10,7 +10,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set DeleteEntityServiceHandler::GetProvidedFeatures() @@ -43,4 +43,4 @@ namespace SimulationInterfacesROS2 }); return AZStd::nullopt; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h index 7a48eb243e..a98f6d0a53 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/DeleteEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class DeleteEntityServiceHandler : public ROS2ServiceBase @@ -34,4 +34,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp similarity index 97% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp index 9640ca33b7..66f2d41539 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -11,7 +11,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set GetEntitiesServiceHandler::GetProvidedFeatures() { @@ -61,4 +61,4 @@ namespace SimulationInterfacesROS2 return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h index c4e96a3430..a2f46ad4de 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class GetEntitiesServiceHandler : public ROS2ServiceBase { @@ -33,4 +33,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp similarity index 98% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index 1afb40a628..9c3842ea24 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set GetEntitiesStatesServiceHandler::GetProvidedFeatures() { @@ -82,4 +82,4 @@ namespace SimulationInterfacesROS2 return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h index 81e857e816..ca4228b486 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntitiesStatesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class GetEntitiesStatesServiceHandler : public ROS2ServiceBase @@ -34,4 +34,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp similarity index 96% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp index 479e5c8fa6..00b50f013f 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -11,7 +11,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set GetEntityStateServiceHandler::GetProvidedFeatures() @@ -51,4 +51,4 @@ namespace SimulationInterfacesROS2 return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h index bc9eb929c2..501109567a 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class GetEntityStateServiceHandler : public ROS2ServiceBase @@ -33,4 +33,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp similarity index 84% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 4084fbefe9..5a4aea23d6 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -10,9 +10,9 @@ #include #include #include -#include +#include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() @@ -24,9 +24,9 @@ namespace SimulationInterfacesROS2 AZStd::optional GetSimulationFeaturesServiceHandler::HandleServiceRequest( const std::shared_ptr header, const Request& request) { - // call bus to get simulation features in SimulationInterfacesROS2 Gem side + // call bus to get simulation features in ROS2SimulationInterfaces Gem side AZStd::unordered_set ros2Interfaces; - SimulationInterfacesROS2RequestBus::BroadcastResult(ros2Interfaces, &SimulationInterfacesROS2Requests::GetSimulationFeatures); + ROS2SimulationInterfacesRequestBus::BroadcastResult(ros2Interfaces, &ROS2SimulationInterfacesRequests::GetSimulationFeatures); // call bus to get simulation features on SimulationInterfaces Gem side AZStd::unordered_set o3deInterfaces; SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::BroadcastResult( @@ -48,4 +48,4 @@ namespace SimulationInterfacesROS2 return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h index 67c53f6bb3..568ade2e66 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class GetSimulationFeaturesServiceHandler : public ROS2ServiceBase @@ -34,4 +34,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp similarity index 94% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp index 6f4319b3a8..ab2066583e 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp @@ -11,7 +11,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set GetSimulationStateServiceHandler::GetProvidedFeatures() { @@ -29,4 +29,4 @@ namespace SimulationInterfacesROS2 response.state.state = currentState; return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h similarity index 93% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h index 72e3814314..2ca83acdbc 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h @@ -13,7 +13,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class GetSimulationStateServiceHandler : public ROS2ServiceBase @@ -33,4 +33,4 @@ namespace SimulationInterfacesROS2 AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp similarity index 96% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp index 4c1969ad05..c2fa782ba3 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -9,7 +9,7 @@ #include "GetSpawnablesServiceHandler.h" #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set GetSpawnablesServiceHandler::GetProvidedFeatures() @@ -51,4 +51,4 @@ namespace SimulationInterfacesROS2 return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h index b17c7b51bb..e2284ce247 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/GetSpawnablesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class GetSpawnablesServiceHandler : public ROS2ServiceBase @@ -34,4 +34,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h similarity index 98% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h rename to Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h index 64c50bbdc3..6e095a2958 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/ROS2ServiceBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h @@ -16,7 +16,7 @@ #include #include -namespace SimulationInterfacesROS2 +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 @@ -89,4 +89,4 @@ namespace SimulationInterfacesROS2 ServiceHandle m_serviceHandle; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp similarity index 98% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index 9b69a0746d..70b6ec946d 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -16,7 +16,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set ResetSimulationServiceHandler::GetProvidedFeatures() @@ -114,4 +114,4 @@ namespace SimulationInterfacesROS2 } return AZStd::nullopt; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h index 4d681bf757..1bb965fd25 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/ResetSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h @@ -13,7 +13,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class ResetSimulationServiceHandler : public ROS2ServiceBase { @@ -32,4 +32,4 @@ namespace SimulationInterfacesROS2 AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp similarity index 96% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp index e0b9c17fbe..303b566c67 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -10,7 +10,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set SetEntityStateServiceHandler::GetProvidedFeatures() @@ -42,4 +42,4 @@ namespace SimulationInterfacesROS2 return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h similarity index 92% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h index f176f574cf..c120af23a2 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class SetEntityStateServiceHandler : public ROS2ServiceBase @@ -34,4 +34,4 @@ namespace SimulationInterfacesROS2 private: }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp similarity index 95% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp index 3422e6141a..b1642b32ff 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -11,7 +11,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set SetSimulationStateServiceHandler::GetProvidedFeatures() { @@ -38,4 +38,4 @@ namespace SimulationInterfacesROS2 } return response; } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h similarity index 93% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h index 5672c39a43..9222be60e7 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h @@ -13,7 +13,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class SetSimulationStateServiceHandler : public ROS2ServiceBase @@ -33,4 +33,4 @@ namespace SimulationInterfacesROS2 AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp similarity index 98% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp index f927e95c80..df46d68550 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -12,7 +12,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set SpawnEntityServiceHandler::GetProvidedFeatures() @@ -86,4 +86,4 @@ namespace SimulationInterfacesROS2 return AZStd::regex_match(frameName, frameRegex); } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h similarity index 93% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h index 5a8ef14578..8fa8678c33 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h @@ -12,7 +12,7 @@ #include #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class SpawnEntityServiceHandler : public ROS2ServiceBase { @@ -37,4 +37,4 @@ namespace SimulationInterfacesROS2 }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp similarity index 96% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.cpp rename to Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp index afde3ad772..ce6a9d6da4 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp @@ -11,7 +11,7 @@ #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZStd::unordered_set StepSimulationServiceHandler::GetProvidedFeatures() @@ -53,4 +53,4 @@ namespace SimulationInterfacesROS2 AZ::TickBus::Handler::BusDisconnect(); } } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h similarity index 93% rename from Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.h rename to Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h index 4bc3dc654c..a41d5649df 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Services/StepSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h @@ -12,7 +12,7 @@ #include #include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { class StepSimulationServiceHandler @@ -38,4 +38,4 @@ namespace SimulationInterfacesROS2 void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp index b9a69a3718..e8262cf970 100644 --- a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -14,6 +14,7 @@ #include "Clients/SimulationFeaturesAggregator.h" #include "Clients/SimulationManager.h" #include +#include namespace SimulationInterfaces { @@ -30,6 +31,7 @@ namespace SimulationInterfaces SimulationEntitiesManager::CreateDescriptor(), SimulationManager::CreateDescriptor(), SimulationFeaturesAggregator::CreateDescriptor(), + ROS2SimulationInterfaces::ROS2SimulationInterfacesSystemComponent::CreateDescriptor(), }); } @@ -39,6 +41,7 @@ namespace SimulationInterfaces azrtti_typeid(), azrtti_typeid(), azrtti_typeid(), + azrtti_typeid(), }; } } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp similarity index 56% rename from Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp rename to Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp index 1c22a235d4..b56503c1b2 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.cpp @@ -6,82 +6,82 @@ * */ -#include "SimulationInterfacesROS2EditorSystemComponent.h" +#include "ROS2SimulationInterfacesEditorSystemComponent.h" #include -#include +#include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { AZ_COMPONENT_IMPL( - SimulationInterfacesROS2EditorSystemComponent, - "SimulationInterfacesROS2EditorSystemComponent", - SimulationInterfacesROS2EditorSystemComponentTypeId, + ROS2SimulationInterfacesEditorSystemComponent, + "ROS2SimulationInterfacesEditorSystemComponent", + ROS2SimulationInterfacesEditorSystemComponentTypeId, BaseSystemComponent); - void SimulationInterfacesROS2EditorSystemComponent::Reflect(AZ::ReflectContext* context) + void ROS2SimulationInterfacesEditorSystemComponent::Reflect(AZ::ReflectContext* context) { if (auto serializeContext = azrtti_cast(context)) { - serializeContext->Class()->Version(0); + serializeContext->Class()->Version(0); } } - SimulationInterfacesROS2EditorSystemComponent::SimulationInterfacesROS2EditorSystemComponent() + ROS2SimulationInterfacesEditorSystemComponent::ROS2SimulationInterfacesEditorSystemComponent() : m_nodeHandler( [this](std::shared_ptr node) { if (!m_systemComponentActivated) { - SimulationInterfacesROS2SystemComponent::Activate(); + ROS2SimulationInterfacesSystemComponent::Activate(); m_systemComponentActivated = true; } else { - SimulationInterfacesROS2SystemComponent::Deactivate(); + ROS2SimulationInterfacesSystemComponent::Deactivate(); m_systemComponentActivated = false; } }) { } - SimulationInterfacesROS2EditorSystemComponent::~SimulationInterfacesROS2EditorSystemComponent() = default; + ROS2SimulationInterfacesEditorSystemComponent::~ROS2SimulationInterfacesEditorSystemComponent() = default; - void SimulationInterfacesROS2EditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + void ROS2SimulationInterfacesEditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) { BaseSystemComponent::GetProvidedServices(provided); - provided.push_back(AZ_CRC_CE("SimulationInterfacesROS2EditorService")); + provided.push_back(AZ_CRC_CE("ROS2SimulationInterfacesEditorService")); } - void SimulationInterfacesROS2EditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + void ROS2SimulationInterfacesEditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) { BaseSystemComponent::GetIncompatibleServices(incompatible); - incompatible.push_back(AZ_CRC_CE("SimulationInterfacesROS2EditorService")); + incompatible.push_back(AZ_CRC_CE("ROS2SimulationInterfacesEditorService")); } - void SimulationInterfacesROS2EditorSystemComponent::GetRequiredServices( + void ROS2SimulationInterfacesEditorSystemComponent::GetRequiredServices( [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { BaseSystemComponent::GetRequiredServices(required); required.push_back(AZ_CRC_CE("ROS2EditorService")); } - void SimulationInterfacesROS2EditorSystemComponent::GetDependentServices( + void ROS2SimulationInterfacesEditorSystemComponent::GetDependentServices( [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) { BaseSystemComponent::GetDependentServices(dependent); } - void SimulationInterfacesROS2EditorSystemComponent::Activate() + void ROS2SimulationInterfacesEditorSystemComponent::Activate() { AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); ROS2::ROS2Interface::Get()->ConnectOnNodeChanged(m_nodeHandler); } - void SimulationInterfacesROS2EditorSystemComponent::Deactivate() + void ROS2SimulationInterfacesEditorSystemComponent::Deactivate() { m_nodeHandler.Disconnect(); AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); } -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h similarity index 68% rename from Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h rename to Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h index 0add61d690..8a395e31d5 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h +++ b/Gems/SimulationInterfaces/Code/Source/Tools/ROS2SimulationInterfacesEditorSystemComponent.h @@ -11,25 +11,25 @@ #include #include -#include +#include #include -namespace SimulationInterfacesROS2 +namespace ROS2SimulationInterfaces { - /// System component for SimulationInterfacesROS2 editor - class SimulationInterfacesROS2EditorSystemComponent - : public SimulationInterfacesROS2SystemComponent + /// System component for ROS2SimulationInterfaces editor + class ROS2SimulationInterfacesEditorSystemComponent + : public ROS2SimulationInterfacesSystemComponent , protected AzToolsFramework::EditorEvents::Bus::Handler { - using BaseSystemComponent = SimulationInterfacesROS2SystemComponent; + using BaseSystemComponent = ROS2SimulationInterfacesSystemComponent; public: - AZ_COMPONENT_DECL(SimulationInterfacesROS2EditorSystemComponent); + AZ_COMPONENT_DECL(ROS2SimulationInterfacesEditorSystemComponent); static void Reflect(AZ::ReflectContext* context); - SimulationInterfacesROS2EditorSystemComponent(); - ~SimulationInterfacesROS2EditorSystemComponent(); + ROS2SimulationInterfacesEditorSystemComponent(); + ~ROS2SimulationInterfacesEditorSystemComponent(); private: static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); @@ -44,4 +44,4 @@ namespace SimulationInterfacesROS2 bool m_systemComponentActivated = false; ROS2::ROS2Requests::NodeChangedEvent::Handler m_nodeHandler; }; -} // namespace SimulationInterfacesROS2 +} // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp index 4311a8be9f..b82b9126b5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp @@ -6,6 +6,7 @@ * */ +#include "ROS2SimulationInterfacesEditorSystemComponent.h" #include "SimulationEntitiesManagerEditor.h" #include "SimulationFeaturesAggregatorEditor.h" #include "SimulationManagerEditor.h" @@ -27,6 +28,7 @@ namespace SimulationInterfaces SimulationEntitiesManagerEditor::CreateDescriptor(), SimulationManagerEditor::CreateDescriptor(), SimulationFeaturesAggregatorEditor::CreateDescriptor(), + ROS2SimulationInterfaces::ROS2SimulationInterfacesEditorSystemComponent::CreateDescriptor(), }); } @@ -39,13 +41,10 @@ namespace SimulationInterfaces return AZ::ComponentTypeList{ azrtti_typeid(), azrtti_typeid(), + azrtti_typeid(), }; } }; } // namespace SimulationInterfaces -#if defined(O3DE_GEM_NAME) -AZ_DECLARE_MODULE_CLASS(AZ_JOIN(Gem_, O3DE_GEM_NAME, _Editor), SimulationInterfaces::SimulationInterfacesEditorModule) -#else AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfaces_Editor, SimulationInterfaces::SimulationInterfacesEditorModule) -#endif diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp similarity index 87% rename from Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp rename to Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp index e72b4481e3..83dec3826b 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp @@ -10,7 +10,7 @@ #include #include -namespace SimulationInterfacesROS2::RegistryUtilities +namespace ROS2SimulationInterfaces::RegistryUtilities { AZStd::string GetName(AZStd::string serviceType) { @@ -21,4 +21,4 @@ namespace SimulationInterfacesROS2::RegistryUtilities settingsRegistry->Get(output, setRegPath.String()); return output; } -} // namespace SimulationInterfacesROS2::RegistryUtilities +} // namespace ROS2SimulationInterfaces::RegistryUtilities diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h similarity index 75% rename from Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h rename to Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h index cb2b92b823..2bedd7f224 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/RegistryUtils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h @@ -9,12 +9,12 @@ #include -namespace SimulationInterfacesROS2::RegistryUtilities +namespace ROS2SimulationInterfaces::RegistryUtilities { // prefix for settings registry related to ros2 topics names - inline constexpr const char* RegistryPrefix = "/SimulationInterfacesROS2"; + 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(AZStd::string serviceType); -} // namespace SimulationInterfacesROS2::RegistryUtilities +} // namespace ROS2SimulationInterfaces::RegistryUtilities diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h similarity index 97% rename from Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h rename to Gems/SimulationInterfaces/Code/Source/Utils/Utils.h index 9b5bc61f1c..e04413edd0 100644 --- a/Gems/SimulationInterfacesROS2/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h @@ -13,7 +13,7 @@ #include #include -namespace SimulationInterfacesROS2::Utils +namespace ROS2SimulationInterfaces::Utils { template AZ::Outcome GetEntityFiltersFromRequest(const RequestType& request) @@ -59,4 +59,4 @@ namespace SimulationInterfacesROS2::Utils } return AZ::Success(AZStd::move(filter)); } -} // namespace SimulationInterfacesROS2::Utils +} // namespace ROS2SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp new file mode 100644 index 0000000000..980bd781d5 --- /dev/null +++ b/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp @@ -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 + * + */ + +#include + +AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp similarity index 99% rename from Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp rename to Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp index 2027a82761..8708bc73f7 100644 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -23,7 +23,7 @@ #include #include -#include "Clients/SimulationInterfacesROS2SystemComponent.h" +#include "Clients/ROS2SimulationInterfacesSystemComponent.h" #include "Mocks/SimulationEntityManagerMock.h" #include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h" #include "Mocks/SimulationManagerMock.h" @@ -57,13 +57,13 @@ namespace UnitTest void SimulationInterfaceROS2TestEnvironment::AddGemsAndComponents() { - using namespace SimulationInterfacesROS2; + using namespace ROS2SimulationInterfaces; AddActiveGems(AZStd::to_array({ "ROS2" })); AddDynamicModulePaths({ "ROS2" }); AddComponentDescriptors(AZStd::initializer_list{ - SimulationInterfacesROS2SystemComponent::CreateDescriptor(), + ROS2SimulationInterfacesSystemComponent::CreateDescriptor(), }); - AddRequiredComponents(AZStd::to_array({ SimulationInterfacesROS2SystemComponent::TYPEINFO_Uuid() })); + AddRequiredComponents(AZStd::to_array({ ROS2SimulationInterfacesSystemComponent::TYPEINFO_Uuid() })); } AZ::ComponentApplication* SimulationInterfaceROS2TestEnvironment::CreateApplicationInstance() diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h similarity index 100% rename from Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h rename to Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h similarity index 100% rename from Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h rename to Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationManagerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationManagerMock.h similarity index 100% rename from Gems/SimulationInterfacesROS2/Code/Tests/Tools/Mocks/SimulationManagerMock.h rename to Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationManagerMock.h diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake b/Gems/SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake similarity index 51% rename from Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake rename to Gems/SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake index 7fcb5d5c53..6e7974cb19 100644 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_tests_files.cmake +++ b/Gems/SimulationInterfaces/Code/ros2_simulationinterfaces_tests_files.cmake @@ -1,3 +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 Tests/Tools/Mocks/SimulationManagerMock.h diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake index 186160f3d0..ae0596f11a 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake @@ -11,4 +11,5 @@ set(FILES Include/SimulationInterfaces/SimulationInterfacesTypeIds.h Include/SimulationInterfaces/SimulationMangerRequestBus.h Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h + Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h ) diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake index ed94889986..4fe290c118 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_editor_private_files.cmake @@ -11,4 +11,6 @@ set(FILES 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_private_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake index 6c10d2ac8d..52f280a259 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_private_files.cmake @@ -15,4 +15,42 @@ set(FILES 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/SimulationInterfacesROS2/.clang-format b/Gems/SimulationInterfacesROS2/.clang-format deleted file mode 100644 index fddf244031..0000000000 --- a/Gems/SimulationInterfacesROS2/.clang-format +++ /dev/null @@ -1,61 +0,0 @@ -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/SimulationInterfacesROS2/.gitignore b/Gems/SimulationInterfacesROS2/.gitignore deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/Gems/SimulationInterfacesROS2/CMakeLists.txt b/Gems/SimulationInterfacesROS2/CMakeLists.txt deleted file mode 100644 index c221acf540..0000000000 --- a/Gems/SimulationInterfacesROS2/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ - -o3de_gem_setup("SimulationInterfacesROS2") - -ly_add_external_target_path(${CMAKE_CURRENT_SOURCE_DIR}/3rdParty) - -add_subdirectory(Code) diff --git a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt b/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt deleted file mode 100644 index 57e0ce3601..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/CMakeLists.txt +++ /dev/null @@ -1,262 +0,0 @@ - -# 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/SimulationInterfacesROS2/Code/Platform/ or -# //Gems/SimulationInterfacesROS2/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_SIMULATIONINTERFACESROS2_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 - simulationinterfacesros2_api_files.cmake - ${pal_dir}/simulationinterfacesros2_api_files.cmake - INCLUDE_DIRECTORIES - INTERFACE - Include - BUILD_DEPENDENCIES - INTERFACE - AZ::AzCore - Gem::SimulationInterfaces.API -) - -# 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 - simulationinterfacesros2_private_files.cmake - ${pal_dir}/simulationinterfacesros2_private_files.cmake - TARGET_PROPERTIES - O3DE_PRIVATE_TARGET TRUE - INCLUDE_DIRECTORIES - PRIVATE - Include - Source - BUILD_DEPENDENCIES - PRIVATE - Gem::SimulationInterfaces.API - Gem::ROS2.Static - PUBLIC - AZ::AzCore - AZ::AzFramework -) - -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 - simulationinterfacesros2_shared_files.cmake - ${pal_dir}/simulationinterfacesros2_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/SimulationInterfacesROS2Module.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 - simulationinterfacesros2_editor_api_files.cmake - ${pal_dir}/simulationinterfacesros2_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 - simulationinterfacesros2_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 - simulationinterfacesros2_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/SimulationInterfacesROS2EditorModule.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_SIMULATIONINTERFACESROS2_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 - simulationinterfacesros2_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_SIMULATIONINTERFACESROS2_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 - simulationinterfacesros2_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 - 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}.Editor.Tests - ) - endif() - endif() -endif() diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake deleted file mode 100644 index b561b79afb..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Android/PAL_android.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake deleted file mode 100644 index 557799ce8d..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Android -# i.e. ../Source/Android/SimulationInterfacesROS2Android.cpp -# ../Source/Android/SimulationInterfacesROS2Android.h -# ../Include/Android/SimulationInterfacesROS2Android.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake deleted file mode 100644 index 557799ce8d..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Android/simulationinterfacesros2_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Android -# i.e. ../Source/Android/SimulationInterfacesROS2Android.cpp -# ../Source/Android/SimulationInterfacesROS2Android.h -# ../Include/Android/SimulationInterfacesROS2Android.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake deleted file mode 100644 index 60cd4e35db..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/PAL_linux.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED TRUE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_editor_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake deleted file mode 100644 index 15f937f53a..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Linux -# i.e. ../Source/Linux/SimulationInterfacesROS2Linux.cpp -# ../Source/Linux/SimulationInterfacesROS2Linux.h -# ../Include/Linux/SimulationInterfacesROS2Linux.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake deleted file mode 100644 index 15f937f53a..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Linux/simulationinterfacesros2_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Linux -# i.e. ../Source/Linux/SimulationInterfacesROS2Linux.cpp -# ../Source/Linux/SimulationInterfacesROS2Linux.h -# ../Include/Linux/SimulationInterfacesROS2Linux.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake deleted file mode 100644 index b561b79afb..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/PAL_mac.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_editor_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake deleted file mode 100644 index 67b288e014..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Mac -# i.e. ../Source/Mac/SimulationInterfacesROS2Mac.cpp -# ../Source/Mac/SimulationInterfacesROS2Mac.h -# ../Include/Mac/SimulationInterfacesROS2Mac.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake deleted file mode 100644 index 67b288e014..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Mac/simulationinterfacesros2_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Mac -# i.e. ../Source/Mac/SimulationInterfacesROS2Mac.cpp -# ../Source/Mac/SimulationInterfacesROS2Mac.h -# ../Include/Mac/SimulationInterfacesROS2Mac.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake deleted file mode 100644 index b561b79afb..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/PAL_windows.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_editor_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake deleted file mode 100644 index 28928dc78c..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Windows -# i.e. ../Source/Windows/SimulationInterfacesROS2Windows.cpp -# ../Source/Windows/SimulationInterfacesROS2Windows.h -# ../Include/Windows/SimulationInterfacesROS2Windows.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake deleted file mode 100644 index 28928dc78c..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/Windows/simulationinterfacesros2_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Windows -# i.e. ../Source/Windows/SimulationInterfacesROS2Windows.cpp -# ../Source/Windows/SimulationInterfacesROS2Windows.h -# ../Include/Windows/SimulationInterfacesROS2Windows.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake deleted file mode 100644 index b561b79afb..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/PAL_ios.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_SUPPORTED TRUE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_SIMULATIONINTERFACESROS2_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake deleted file mode 100644 index f5526eeb37..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake deleted file mode 100644 index 2afd8fda21..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for iOS -# i.e. ../Source/iOS/SimulationInterfacesROS2iOS.cpp -# ../Source/iOS/SimulationInterfacesROS2iOS.h -# ../Include/iOS/SimulationInterfacesROS2iOS.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake deleted file mode 100644 index 2afd8fda21..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Platform/iOS/simulationinterfacesros2_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for iOS -# i.e. ../Source/iOS/SimulationInterfacesROS2iOS.cpp -# ../Source/iOS/SimulationInterfacesROS2iOS.h -# ../Include/iOS/SimulationInterfacesROS2iOS.h - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp deleted file mode 100644 index 6c19a010b3..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Source/Clients/SimulationInterfacesROS2Module.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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 "SimulationInterfacesROS2SystemComponent.h" -#include -#include - -namespace SimulationInterfacesROS2 -{ - class SimulationInterfacesROS2Module : public SimulationInterfacesROS2ModuleInterface - { - public: - AZ_RTTI(SimulationInterfacesROS2Module, SimulationInterfacesROS2ModuleTypeId, SimulationInterfacesROS2ModuleInterface); - AZ_CLASS_ALLOCATOR(SimulationInterfacesROS2Module, AZ::SystemAllocator); - }; -} // namespace SimulationInterfacesROS2 - -AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfacesROS2, SimulationInterfacesROS2::SimulationInterfacesROS2Module) diff --git a/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp b/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp deleted file mode 100644 index d85b1133cd..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/* - * 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 "SimulationInterfacesROS2ModuleInterface.h" -#include - -#include - -#include - -namespace SimulationInterfacesROS2 -{ - AZ_TYPE_INFO_WITH_NAME_IMPL( - SimulationInterfacesROS2ModuleInterface, "SimulationInterfacesROS2ModuleInterface", SimulationInterfacesROS2ModuleInterfaceTypeId); - AZ_RTTI_NO_TYPE_INFO_IMPL(SimulationInterfacesROS2ModuleInterface, AZ::Module); - AZ_CLASS_ALLOCATOR_IMPL(SimulationInterfacesROS2ModuleInterface, AZ::SystemAllocator); - - SimulationInterfacesROS2ModuleInterface::SimulationInterfacesROS2ModuleInterface() - { - m_descriptors.insert( - m_descriptors.end(), - { - SimulationInterfacesROS2SystemComponent::CreateDescriptor(), - }); - } - - AZ::ComponentTypeList SimulationInterfacesROS2ModuleInterface::GetRequiredSystemComponents() const - { - return AZ::ComponentTypeList{ - azrtti_typeid(), - }; - } -} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h b/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h deleted file mode 100644 index 448e62490d..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Source/SimulationInterfacesROS2ModuleInterface.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * 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 SimulationInterfacesROS2 -{ - class SimulationInterfacesROS2ModuleInterface : public AZ::Module - { - public: - AZ_TYPE_INFO_WITH_NAME_DECL(SimulationInterfacesROS2ModuleInterface) - AZ_RTTI_NO_TYPE_INFO_DECL() - AZ_CLASS_ALLOCATOR_DECL - - SimulationInterfacesROS2ModuleInterface(); - - AZ::ComponentTypeList GetRequiredSystemComponents() const override; - }; -} // namespace SimulationInterfacesROS2 diff --git a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp b/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp deleted file mode 100644 index 23c02f7e27..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Source/Tools/SimulationInterfacesROS2EditorModule.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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 "SimulationInterfacesROS2EditorSystemComponent.h" -#include -#include - -namespace SimulationInterfacesROS2 -{ - class SimulationInterfacesROS2EditorModule : public SimulationInterfacesROS2ModuleInterface - { - public: - AZ_RTTI(SimulationInterfacesROS2EditorModule, SimulationInterfacesROS2EditorModuleTypeId, SimulationInterfacesROS2ModuleInterface); - AZ_CLASS_ALLOCATOR(SimulationInterfacesROS2EditorModule, AZ::SystemAllocator); - - SimulationInterfacesROS2EditorModule() - { - m_descriptors.insert( - m_descriptors.end(), - { - SimulationInterfacesROS2EditorSystemComponent::CreateDescriptor(), - }); - } - - AZ::ComponentTypeList GetRequiredSystemComponents() const override - { - return AZ::ComponentTypeList{ - azrtti_typeid(), - }; - } - }; -} // namespace SimulationInterfacesROS2 - -AZ_DECLARE_MODULE_CLASS(Gem_SimulationInterfacesROS2_Editor, SimulationInterfacesROS2::SimulationInterfacesROS2EditorModule) diff --git a/Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp b/Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp deleted file mode 100644 index 274a990803..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/Tests/Clients/SimulationInterfacesROS2Test.cpp +++ /dev/null @@ -1,4 +0,0 @@ - -#include - -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake deleted file mode 100644 index 5cbf0a08f0..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_api_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Include/SimulationInterfacesROS2/SimulationInterfacesROS2TypeIds.h -) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake deleted file mode 100644 index 8cd37a5cf0..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_api_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - - -set(FILES -) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake deleted file mode 100644 index b3187e11b6..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_private_files.cmake +++ /dev/null @@ -1,5 +0,0 @@ - -set(FILES - Source/Tools/SimulationInterfacesROS2EditorSystemComponent.cpp - Source/Tools/SimulationInterfacesROS2EditorSystemComponent.h -) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake deleted file mode 100644 index fcc9fd1d42..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_editor_shared_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Source/Tools/SimulationInterfacesROS2EditorModule.cpp -) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake deleted file mode 100644 index 0ade664943..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_private_files.cmake +++ /dev/null @@ -1,43 +0,0 @@ - -set(FILES - Source/SimulationInterfacesROS2ModuleInterface.cpp - Source/SimulationInterfacesROS2ModuleInterface.h - Source/Clients/SimulationInterfacesROS2SystemComponent.cpp - Source/Clients/SimulationInterfacesROS2SystemComponent.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/StepSimulationServiceHandler.cpp - Source/Services/StepSimulationServiceHandler.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/Utils/RegistryUtils.cpp - Source/Utils/RegistryUtils.h - Source/Utils/Utils.h -) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake deleted file mode 100644 index e710269e6e..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_shared_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Source/Clients/SimulationInterfacesROS2Module.cpp -) diff --git a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake b/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake deleted file mode 100644 index 095268dc57..0000000000 --- a/Gems/SimulationInterfacesROS2/Code/simulationinterfacesros2_tests_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Tests/Clients/SimulationInterfacesROS2Test.cpp -) diff --git a/Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg b/Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg deleted file mode 100644 index 5c96cccb23..0000000000 --- a/Gems/SimulationInterfacesROS2/Registry/assetprocessor_settings.setreg +++ /dev/null @@ -1,18 +0,0 @@ -{ - "Amazon": { - "AssetProcessor": { - "Settings": { - "ScanFolder SimulationInterfacesROS2/Assets": { - "watch": "@GEMROOT:SimulationInterfacesROS2@/Assets", - "recursive": 1, - "order": 101 - }, - "ScanFolder SimulationInterfacesROS2/Registry": { - "watch": "@GEMROOT:SimulationInterfacesROS2@/Registry", - "recursive": 1, - "order": 102 - } - } - } - } -} diff --git a/Gems/SimulationInterfacesROS2/gem.json b/Gems/SimulationInterfacesROS2/gem.json deleted file mode 100644 index 24745d49f4..0000000000 --- a/Gems/SimulationInterfacesROS2/gem.json +++ /dev/null @@ -1,34 +0,0 @@ -{ - "gem_name": "SimulationInterfacesROS2", - "version": "1.0.0", - "display_name": "SimulationInterfacesROS2", - "license": "Apache-2.0", - "license_url": "https://opensource.org/licenses/Apache-2.0", - "origin": "RobotecAI", - "origin_url": "https://robotec.ai", - "type": "Code", - "summary": "This gem provides ROS 2 interface for SimulationInterfaces gem.", - "canonical_tags": [ - "Gem" - ], - "user_tags": [ - "SimulationInterfacesROS2", - "SimulationInterfaces", - "ROS2", - "ROS 2" - ], - "platforms": [ - "" - ], - "icon_path": "preview.png", - "requirements": "ROS2, SimulationInterfaces", - "documentation_url": "", - "dependencies": [ - "SimulationInterfaces>=1.0.0", - "ROS2>=3.3.0" - ], - "repo_uri": "", - "compatible_engines": [], - "engine_api_dependencies": [], - "restricted": "SimulationInterfacesROS2" -} diff --git a/Gems/SimulationInterfacesROS2/preview.png b/Gems/SimulationInterfacesROS2/preview.png deleted file mode 100644 index b321ae48bc..0000000000 --- a/Gems/SimulationInterfacesROS2/preview.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:248e3ffe1fc9ffc02afb2ba8914e222a5a5d13ac45a48b98c95ee062e959a94c -size 4475 From d6bd8b54df432b75b4ddc3d4f9706280d1eb39c3 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Fri, 11 Apr 2025 13:54:25 +0200 Subject: [PATCH 23/35] Simulation Interfaces hotfixes (#879) * Fixed wrong pose in setEntityState Signed-off-by: Norbert Prokopiuk * Added return message in case of invalid request Signed-off-by: Norbert Prokopiuk --------- Signed-off-by: Norbert Prokopiuk --- .../Clients/SimulationEntitiesManager.cpp | 38 ++++++++++--------- .../ResetSimulationServiceHandler.cpp | 22 +++++++---- 2 files changed, 35 insertions(+), 25 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 47de29bf26..0decd058ce 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -160,7 +160,7 @@ namespace SimulationInterfaces // cache the initial state EntityState initialState{}; - initialState.m_pose = entity->GetTransform()->GetLocalTM(); + initialState.m_pose = entity->GetTransform()->GetWorldTM(); if (rigidBody) { initialState.m_twist_linear = rigidBody->GetLinearVelocity(); @@ -221,10 +221,10 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{//simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, + AZStd::unordered_set{ // simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, - //simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, - //simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, + // simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, + // simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING, simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, simulation_interfaces::msg::SimulatorFeatures::DELETING, @@ -444,6 +444,8 @@ namespace SimulationInterfaces return; } const AZ::EntityId entityId = entityAndDescendants.front(); + AZ::EntityId parentEntityId = AZ::EntityId{ AZ::EntityId::InvalidEntityId }; + AZ::TransformBus::EventResult(parentEntityId, entityId, &AZ::TransformBus::Events::GetParentId); if (state.m_pose.IsOrthogonal()) { // disable simulation for all entities @@ -451,8 +453,17 @@ namespace SimulationInterfaces { Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics); } - - AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose); + if (parentEntityId.IsValid()) + { + AZ::Transform parentTransform; + AZ::TransformBus::EventResult(parentTransform, parentEntityId, &AZ::TransformBus::Events::GetWorldTM); + auto transformToSet = parentTransform.GetInverse() * state.m_pose; + AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, transformToSet); + } + else + { + AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalTM, state.m_pose); + } for (const auto& descendant : entityAndDescendants) { @@ -679,20 +690,13 @@ namespace SimulationInterfaces AZStd::string entityName = AZStd::string::format("%s_%s", name.c_str(), entity->GetName().c_str()); entity->SetName(entityName); } + const AZ::Entity* root = *view.begin(); - // get the first entity - if (view.size()>1) + auto* transformInterface = root->FindComponent(); + if (transformInterface) { - const AZ::Entity* firstEntity = view[1]; - AZ_Assert(firstEntity, "First entity is not available"); - auto* transformInterface = firstEntity->FindComponent(); - if (transformInterface) - { - transformInterface->SetWorldTM(initialPose); - } + transformInterface->SetWorldTM(initialPose); } - - }; optionalArgs.m_completionCallback = [this, uri](AzFramework::EntitySpawnTicket::Id ticketId, AzFramework::SpawnableConstEntityContainerView view) diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index 70b6ec946d..5a2ff4e0c9 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -7,14 +7,15 @@ */ #include "ResetSimulationServiceHandler.h" +#include #include +#include +#include #include #include #include #include -#include -#include -#include +#include namespace ROS2SimulationInterfaces { @@ -24,7 +25,7 @@ namespace ROS2SimulationInterfaces return AZStd::unordered_set{ SimulationFeatures::SIMULATION_RESET, SimulationFeatures::SIMULATION_RESET_TIME, SimulationFeatures::SIMULATION_RESET_STATE, - SimulationFeatures::SIMULATION_RESET_SPAWNED}; + SimulationFeatures::SIMULATION_RESET_SPAWNED }; } AZStd::optional ResetSimulationServiceHandler::HandleServiceRequest( @@ -63,7 +64,7 @@ namespace ROS2SimulationInterfaces if (request.scope == Request::SCOPE_TIME) { - auto * interface = ROS2::ROS2Interface::Get(); + auto* interface = ROS2::ROS2Interface::Get(); AZ_Assert(interface, "ROS2Interface is not available"); auto& clock = interface->GetSimulationClock(); @@ -82,7 +83,7 @@ namespace ROS2SimulationInterfaces { Response response; response.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; - const auto & errorMessage = results.GetError(); + const auto& errorMessage = results.GetError(); response.result.error_message = std::string(errorMessage.c_str(), errorMessage.size()); return response; } @@ -108,10 +109,15 @@ namespace ROS2SimulationInterfaces SendResponse(response); }; SimulationInterfaces::SimulationManagerRequestBus::Broadcast( - &SimulationInterfaces::SimulationManagerRequests::ReloadLevel, levelReloadCompletion); + &SimulationInterfaces::SimulationManagerRequests::ReloadLevel, levelReloadCompletion); return AZStd::nullopt; } - return AZStd::nullopt; + + // no case matched, return response that request was invalid + Response invalidResponse; + invalidResponse.result.result = simulation_interfaces::msg::Result::RESULT_NOT_FOUND; + invalidResponse.result.error_message = "Passed unknown scope"; + return invalidResponse; } } // namespace ROS2SimulationInterfaces From 5d404545b7030daf8ae78990d55d99e7c48992d2 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Mon, 14 Apr 2025 10:02:15 +0200 Subject: [PATCH 24/35] Fix for wrong simulation state after reset (#883) Signed-off-by: Norbert Prokopiuk --- .../Code/Source/Clients/SimulationManager.cpp | 27 ++++++++++++------- .../Code/Source/Clients/SimulationManager.h | 2 ++ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index 9d8e52d589..fa3adb5750 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -84,6 +84,20 @@ namespace SimulationInterfaces void SimulationManager::Init() { } + 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() { @@ -104,16 +118,7 @@ namespace SimulationInterfaces AZ::SystemTickBus::QueueFunction( [this]() { - // 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()) - { - SetSimulationPaused(true); - } - else - { - SetSimulationState(simulation_interfaces::msg::SimulationState::STATE_PLAYING); - } + InitializeSimulationState(); }); } @@ -224,6 +229,8 @@ namespace SimulationInterfaces m_reloadLevelCallback(); m_reloadLevelCallback = nullptr; } + // reset of the simulation, assign the same state as at the beginning + InitializeSimulationState(); AzFramework::LevelSystemLifecycleNotificationBus::Handler::BusDisconnect(); } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index 5eea4de0b1..bc8f85b5af 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -68,6 +68,8 @@ namespace SimulationInterfaces SimulationState m_simulationState{ simulation_interfaces::msg::SimulationState::STATE_STOPPED }; // default simulation state based on standard + void InitializeSimulationState(); + private: bool IsTransitionForbidden(SimulationState requestedState); // forbidden transition between state, first is current state, second is desire state From 7b66b1bfa81ce0c168a93e34f3e1411990a51a37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Apr 2025 12:30:43 +0100 Subject: [PATCH 25/35] Fix for articulations (#887) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix for articulations - added alternative registration path for SimulationBodies that are not yet configured. --------- Signed-off-by: Michał Pełka --- .../Clients/SimulationEntitiesManager.cpp | 210 +++++++++++------- .../Clients/SimulationEntitiesManager.h | 21 ++ 2 files changed, 149 insertions(+), 82 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 0decd058ce..4e30c2a02d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -28,11 +28,13 @@ #include #include #include +#include #include #include #include #include + namespace SimulationInterfaces { void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) @@ -112,61 +114,97 @@ namespace SimulationInterfaces return scene; } + + 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_twist_linear = rigidBody->GetLinearVelocity(); + initialState.m_twist_angular = 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) { - auto* scene = GetSceneHelper(sceneHandle); - if (scene == nullptr) - { - return; - } - auto* body = scene->GetSimulatedBodyFromHandle(bodyHandle); - AZ_Assert(body, "Simulated body is not available."); - 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(); - AZ::Entity* entity = nullptr; - AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId); - AZ_Assert(entity, "Entity is not available."); - // check if entity is not spawned by this component - const auto ticketId = entity->GetEntitySpawnTicketId(); - AZStd::string proposedName{}; - // check if ticket is in the unregistered list - - auto spawnData = m_spawnCompletedCallbacks.find(ticketId); - if (spawnData != m_spawnCompletedCallbacks.end()) - { - proposedName = spawnData->second.m_userProposedName; - } - - const AZStd::string registeredName = this->AddSimulatedEntity(entityId, proposedName); - // call the callback - if (spawnData != m_spawnCompletedCallbacks.end()) - { - // call and remove the callback - spawnData->second.m_completedCb(AZ::Success(registeredName)); - m_spawnCompletedCallbacks.erase(spawnData); - } - - // cache the initial state - EntityState initialState{}; - initialState.m_pose = entity->GetTransform()->GetWorldTM(); - if (rigidBody) - { - initialState.m_twist_linear = rigidBody->GetLinearVelocity(); - initialState.m_twist_angular = rigidBody->GetAngularVelocity(); - } - m_entityIdToInitialState[entityId] = initialState; + 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) @@ -230,10 +268,12 @@ namespace SimulationInterfaces 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(); if (m_simulationBodyAddedHandler.IsConnected()) { @@ -256,6 +296,7 @@ namespace SimulationInterfaces AZStd::string SimulationEntitiesManager::AddSimulatedEntity(AZ::EntityId entityId, const AZStd::string& userProposedName) { + if (!entityId.IsValid()) { return ""; @@ -270,7 +311,6 @@ namespace SimulationInterfaces AZStd::string simulatedEntityName = GetSimulatedEntityName(entityId, userProposedName); m_simulatedEntityToEntityIdMap[simulatedEntityName] = entityId; m_entityIdToSimulatedEntityMap[entityId] = simulatedEntityName; - AZ_Printf("SimulationInterfaces", "Registered entity %s\n", simulatedEntityName.c_str()); return simulatedEntityName; } @@ -683,15 +723,7 @@ namespace SimulationInterfaces } } } - - // change names for all entites - for (auto* entity : view) - { - AZStd::string entityName = AZStd::string::format("%s_%s", name.c_str(), entity->GetName().c_str()); - entity->SetName(entityName); - } const AZ::Entity* root = *view.begin(); - auto* transformInterface = root->FindComponent(); if (transformInterface) { @@ -701,6 +733,8 @@ namespace SimulationInterfaces 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 @@ -708,25 +742,31 @@ namespace SimulationInterfaces auto spawnData = m_spawnCompletedCallbacks.find(ticketId); if (spawnData != m_spawnCompletedCallbacks.end()) { - // call and remove the callback - 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()); - AZ_Error("SimulationInterfaces", false, msg.c_str()); - spawnData->second.m_completedCb(msg); + // 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_Printf("SimulationInterfaces", "Spawning uri %s with ticket id %d\n", uri.c_str(), ticketId); + AZ_Info("SimulationInterfaces", "Spawning uri %s with ticket id %d\n", uri.c_str(), ticketId); SpawnCompletedCbData data; data.m_userProposedName = name; @@ -738,21 +778,22 @@ namespace SimulationInterfaces AZStd::string SimulationEntitiesManager::GetSimulatedEntityName(AZ::EntityId entityId, const AZStd::string& proposedName) const { // Get O3DE entity name - AZStd::string entityName = proposedName; - if (entityName.empty()) + 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()); } - // Generate unique simulated entity name - AZStd::string simulatedEntityName = entityName; - // check if name is unique - auto otherEntityIt = m_simulatedEntityToEntityIdMap.find(simulatedEntityName); - if (otherEntityIt != m_simulatedEntityToEntityIdMap.end()) + + // check if name is still not unique, if not, add EntityId to name + if(m_simulatedEntityToEntityIdMap.contains(newName)) { - // name is not unique, add entityId to name - simulatedEntityName = AZStd::string::format("%s_%s", entityName.c_str(), entityId.ToString().c_str()); + newName = AZStd::string::format("%s_%s", newName.c_str(), entityId.ToString().c_str()); } - return simulatedEntityName; + return newName; } void SimulationEntitiesManager::ResetAllEntitiesToInitialState() @@ -765,4 +806,9 @@ namespace SimulationInterfaces 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 index 3146aaa01f..244033f2dd 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -21,6 +21,7 @@ namespace SimulationInterfaces class SimulationEntitiesManager : public AZ::Component , protected SimulationEntityManagerRequestBus::Handler + , protected AZ::TickBus::Handler { public: AZ_COMPONENT_DECL(SimulationEntitiesManager); @@ -58,8 +59,22 @@ namespace SimulationInterfaces void Activate() override; void Deactivate() override; + // AZ::TickBus::Handler interface implementation + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + private: + //! Registers a new simulated body to the simulation interface. + //! Note that the body handle will be registered under unique name + //! Note that body need to be configured to be registered + //! \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 @@ -76,22 +91,28 @@ namespace SimulationInterfaces //! Set the state of the entity and their descendants. void SetEntitiesState(const AZStd::vector& entityAndDescendants, const EntityState& state); + AzPhysics::SceneEvents::OnSimulationBodyAdded::Handler m_simulationBodyAddedHandler; AzPhysics::SceneEvents::OnSimulationBodyRemoved::Handler m_simulationBodyRemovedHandler; 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 From 6109f38e8723d306a64ff68457ac829c3205da2e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Apr 2025 15:19:24 +0100 Subject: [PATCH 26/35] Use simulated body request instead rigid body (#888) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Code/Source/Clients/SimulationEntitiesManager.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 4e30c2a02d..3cee2904b7 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -491,7 +491,8 @@ namespace SimulationInterfaces // disable simulation for all entities for (const auto& descendant : entityAndDescendants) { - Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::DisablePhysics); + AzPhysics::SimulatedBodyComponentRequestsBus::Event(descendant, &AzPhysics::SimulatedBodyComponentRequests::DisablePhysics); + } if (parentEntityId.IsValid()) { @@ -507,7 +508,7 @@ namespace SimulationInterfaces for (const auto& descendant : entityAndDescendants) { - Physics::RigidBodyRequestBus::Event(descendant, &Physics::RigidBodyRequests::EnablePhysics); + 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()); } From f0840564809d2dfb43d6c2903fa8573f27e57f42 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Wed, 16 Apr 2025 12:31:50 +0200 Subject: [PATCH 27/35] Added missing #pragma once Signed-off-by: Norbert Prokopiuk --- Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h index 5e2f1d4e8d..4eb85efdf2 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/TestFixture.h @@ -6,6 +6,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ +#pragma once #include #include From f1fbcc87abf7a8996161bdbfa99a99e298809092 Mon Sep 17 00:00:00 2001 From: Norbert Prokopiuk Date: Wed, 16 Apr 2025 16:02:11 +0200 Subject: [PATCH 28/35] Simulation interfaces - part of review suggestions applied (#889) * Review sugestion based on @norbertprokopiuk comments Signed-off-by: Norbert Prokopiuk * Review sugestion based on part of @adamdbrw comments Signed-off-by: Norbert Prokopiuk * Review sugestion based on part of @PawelLiberadzki comments Signed-off-by: Norbert Prokopiuk * Unififed aliases usage for simulation Interface, dropped const for buses, based on @PawelLiberadzki and @michalpelka comments Signed-off-by: Norbert Prokopiuk * Removed TAG_FILTER enum Signed-off-by: Norbert Prokopiuk --------- Signed-off-by: Norbert Prokopiuk --- Gems/SimulationInterfaces/.gitignore | 0 .../ROS2SimulationInterfacesRequestBus.h | 7 +- .../Include/SimulationInterfaces/Result.h | 12 +- .../SimulationEntityManagerRequestBus.h | 22 +-- .../SimulationFeaturesAggregatorRequestBus.h | 10 +- .../SimulationMangerRequestBus.h | 8 +- .../Include/SimulationInterfaces/TagFilter.h | 6 +- .../Code/Source/Actions/ROS2ActionBase.h | 1 - .../Code/Source/Clients/CommonUtilities.cpp | 2 +- .../Code/Source/Clients/CommonUtilities.h | 2 +- .../Code/Source/Clients/ConsoleCommands.inl | 64 ++++----- .../Clients/SimulationEntitiesManager.cpp | 136 ++++++++---------- .../Clients/SimulationEntitiesManager.h | 20 ++- .../Clients/SimulationFeaturesAggregator.cpp | 8 +- .../Clients/SimulationFeaturesAggregator.h | 11 +- .../Code/Source/Clients/SimulationManager.cpp | 20 +-- .../Code/Source/Clients/SimulationManager.h | 4 +- .../Services/DeleteEntityServiceHandler.cpp | 4 +- .../Services/GetEntitiesServiceHandler.cpp | 4 +- .../GetEntitiesStatesServiceHandler.cpp | 46 +++--- .../Services/GetEntityStateServiceHandler.cpp | 10 +- .../GetSimulationFeaturesServiceHandler.cpp | 13 +- .../GetSimulationFeaturesServiceHandler.h | 3 +- .../GetSimulationStateServiceHandler.h | 1 - .../Services/GetSpawnablesServiceHandler.cpp | 8 +- .../Code/Source/Services/ROS2ServiceBase.h | 1 - .../ResetSimulationServiceHandler.cpp | 4 +- .../Services/ResetSimulationServiceHandler.h | 1 - .../Services/SetEntityStateServiceHandler.cpp | 8 +- .../SetSimulationStateServiceHandler.cpp | 4 +- .../SetSimulationStateServiceHandler.h | 1 - .../Services/SpawnEntityServiceHandler.cpp | 13 +- .../Code/Source/Utils/Utils.h | 18 +-- .../Code/Tests/Tools/InterfacesTest.cpp | 56 ++++---- ...ionFeaturesAggregatorRequestsHandlerMock.h | 6 +- .../Tests/Tools/SimulationInterfaceTests.cpp | 11 +- .../Tests/Tools/SimulationIterfaceAppTest.cpp | 28 ++-- 37 files changed, 268 insertions(+), 305 deletions(-) delete mode 100644 Gems/SimulationInterfaces/.gitignore diff --git a/Gems/SimulationInterfaces/.gitignore b/Gems/SimulationInterfaces/.gitignore deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h index 865ec4ded4..db4a75ac39 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h @@ -8,7 +8,7 @@ #pragma once -#include +#include "ROS2SimulationInterfacesTypeIds.h" #include #include @@ -16,6 +16,7 @@ namespace ROS2SimulationInterfaces { + using SimulationFeatureType = uint8_t; class ROS2SimulationInterfacesRequests { public: @@ -23,9 +24,9 @@ namespace ROS2SimulationInterfaces virtual ~ROS2SimulationInterfacesRequests() = default; //! Returns set of simulation features available in ROS2SimulationInterfaces Gem - //! SimulationFeatures follows definition available at: + //! SimulationFeatureType follows definition available at: //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg - virtual AZStd::unordered_set GetSimulationFeatures() = 0; + virtual AZStd::unordered_set GetSimulationFeatures() = 0; }; class ROS2SimulationInterfacesRequestBusTraits : public AZ::EBusTraits diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h index d7fdeca067..6a9de009db 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h @@ -13,18 +13,18 @@ namespace SimulationInterfaces { //! Result codes to be used in the Result message //! @see Result.msg - using ErrorCodeValue = uint8_t; + using ErrorCodeType = uint8_t; //! A message type to represent the result of a failed operation struct FailedResult { FailedResult() = default; - FailedResult(ErrorCodeValue error_code, const AZStd::string& error_string) - : error_code(error_code) - , error_string(error_string) + FailedResult(ErrorCodeType errorCode, const AZStd::string& errorString) + : m_errorCode(errorCode) + , m_errorString(errorString) { } - ErrorCodeValue error_code; - AZStd::string error_string; + ErrorCodeType m_errorCode; + AZStd::string m_errorString; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index c930b70a47..bf2bca2feb 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -8,7 +8,7 @@ #pragma once -#include +#include "SimulationInterfacesTypeIds.h" #include "Result.h" #include "TagFilter.h" @@ -19,30 +19,33 @@ namespace SimulationInterfaces { - //! # A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. + //! A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates. //! @see EntityFilters.msg struct EntityFilters { - AZStd::string m_filter; //! A posix regular expression to match against entity names - TagFilter m_tags_filter; //! A filter to match against entity tags + //! A posix regular expression to match against entity names, + //! The regular expression syntax is POSIX Extended, + //! @see POSIX_Extended definitions + AZStd::string m_nameFilter; + TagFilter m_tagsFilter; //! A filter to match against entity tags AZStd::shared_ptr - m_bounds_shape; //! A shape to use for filtering entities, null means no bounds filtering - AZ::Transform m_bounds_pose{ AZ::Transform::CreateIdentity() }; + m_boundsShape; //! A shape to use for filtering entities, null means no bounds filtering + AZ::Transform m_boundsPose{ AZ::Transform::CreateIdentity() }; }; //! @see EntityState.msg struct EntityState { AZ::Transform m_pose; //! The pose of the entity - AZ::Vector3 m_twist_linear; //! The linear velocity of the entity (in the entity frame) - AZ::Vector3 m_twist_angular; //! The angular velocity of the entity (in the entity frame) + AZ::Vector3 m_twistLinear; //! The linear velocity of the entity (in the entity frame) + AZ::Vector3 m_twistAngular; //! The angular velocity of the entity (in the entity frame) }; struct Spawnable { AZStd::string m_uri; AZStd::string m_description; - AZStd::string m_bounds_sphere; + AZStd::string m_boundsSphere; }; using EntityNameList = AZStd::vector; @@ -50,6 +53,7 @@ namespace SimulationInterfaces using SpawnableList = AZStd::vector; using DeletionCompletedCb = AZStd::function&)>; using SpawnCompletedCb = AZStd::function&)>; + class SimulationEntityManagerRequests { public: diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h index b5b325d842..805a03b228 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -8,7 +8,7 @@ #pragma once -#include +#include "SimulationInterfacesTypeIds.h" #include #include @@ -16,7 +16,7 @@ namespace SimulationInterfaces { - using SimulationFeatures = uint8_t; + using SimulationFeatureType = uint8_t; class SimulationFeaturesAggregatorRequests { @@ -25,15 +25,15 @@ namespace SimulationInterfaces virtual ~SimulationFeaturesAggregatorRequests() = default; //! Registers simulation features defined by caller - virtual void AddSimulationFeatures(const AZStd::unordered_set& features) = 0; + virtual void AddSimulationFeatures(const AZStd::unordered_set& features) = 0; //! Returns features available in the simulator, list follows definitions at //! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg - virtual const AZStd::unordered_set GetSimulationFeatures() const = 0; + virtual AZStd::unordered_set GetSimulationFeatures() = 0; //! Method checks if feature with given id is available in the simulation //! Method is extension to standard defined in simulation_interfaces - virtual bool HasFeature(SimulationFeatures feature) const = 0; + virtual bool HasFeature(SimulationFeatureType feature) = 0; }; class SimulationFeaturesAggregatorRequestBusTraits : public AZ::EBusTraits diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 63bb03822a..17f45935b8 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -8,25 +8,27 @@ #pragma once -#include +#include "SimulationInterfacesTypeIds.h" +#include "Result.h" #include #include #include #include -#include namespace SimulationInterfaces { using SimulationState = uint8_t; + class SimulationManagerRequests { public: AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId); virtual ~SimulationManagerRequests() = default; - //! Reload level and removal of all spawned simulation entities. using ReloadLevelCallback = AZStd::function; + + //! Reload level and removal of all spawned simulation entities. virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0; //! Set the simulation to paused or unpaused, diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h index cdd0660a05..1a1bb0e0bf 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h @@ -15,14 +15,10 @@ namespace SimulationInterfaces { //! Structure to design a filter for tags //! @see TagsFilter.msg + using TagFilterMode = uint8_t; struct TagFilter { - enum class TagFilterMode - { - FILTER_MODE_ANY, - FILTER_MODE_ALL - }; AZStd::unordered_set m_tags; TagFilterMode m_mode; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h index 6fcacb10bf..d36648f259 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h @@ -11,7 +11,6 @@ #include #include #include -#include #include #include diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp index d50ffb57e1..dfbe95d037 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.cpp @@ -28,4 +28,4 @@ namespace SimulationInterfaces::Utils } return {}; } -} // namespace SimulationInterfaces::Utils \ No newline at end of file +} // namespace SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h index 0d0a96a802..2ca5f2c4ca 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/CommonUtilities.h @@ -17,4 +17,4 @@ namespace SimulationInterfaces::Utils AZStd::string RelPathToUri(AZStd::string_view relPath); AZStd::string UriToRelPath(AZStd::string_view relPath); -} // namespace SimulationInterfaces::Utils \ No newline at end of file +} // namespace SimulationInterfaces::Utils diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl index 620c808163..38dd7d7984 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl @@ -20,10 +20,11 @@ namespace SimulationInterfacesCommands static void simulationinterfaces_GetEntities(const AZ::ConsoleCommandContainer& arguments) { AZ::Outcome entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); + SimulationEntityManagerRequestBus::BroadcastResult( + entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); if (!entities.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str()); return; } @@ -67,13 +68,13 @@ namespace SimulationInterfacesCommands AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntities in radius %f \n", sphereShape); AZ_Printf("SimulationInterfacesConsole", "position %f %f %f \n", position.GetX(), position.GetY(), position.GetZ()); EntityFilters filter; - filter.m_bounds_shape = AZStd::make_shared(sphereShape); + filter.m_boundsShape = AZStd::make_shared(sphereShape); AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); if (!entities.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str()); return; } @@ -93,33 +94,23 @@ namespace SimulationInterfacesCommands const AZStd::string entityName = arguments[0]; AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState %s\n", entityName.c_str()); AZ::Outcome entityStateResult; - SimulationEntityManagerRequestBus::BroadcastResult(entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); + SimulationEntityManagerRequestBus::BroadcastResult( + entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); if (!entityStateResult.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().error_string.c_str()); + AZ_Printf( + "SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().m_errorString.c_str()); return; } - const auto &entityState = entityStateResult.GetValue(); + const auto& entityState = entityStateResult.GetValue(); AZ_Printf("SimulationInterfacesConsole", "Entity %s\n", entityName.c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Pose %s\n", - AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Rotation %s \n", - AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Pose %s\n", AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Rotation %s \n", AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str()); const AZ::Vector3 euler = entityState.m_pose.GetRotation().GetEulerDegrees(); AZ_Printf("SimulationInterfacesConsole", "Rotation (euler) %s\n", AZ::Vector3ToString(euler).c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Twist Linear %s\n", - AZ::Vector3ToString(entityState.m_twist_linear).c_str()); - AZ_Printf( - "SimulationInterfacesConsole", - "Twist Angular %s\n", - AZ::Vector3ToString(entityState.m_twist_angular).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Twist Linear %s\n", AZ::Vector3ToString(entityState.m_twistLinear).c_str()); + AZ_Printf("SimulationInterfacesConsole", "Twist Angular %s\n", AZ::Vector3ToString(entityState.m_twistAngular).c_str()); } static void simulationinterfaces_SetStateXYZ(const AZ::ConsoleCommandContainer& arguments) @@ -143,11 +134,10 @@ namespace SimulationInterfacesCommands if (!result.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().m_errorString.c_str()); return; } AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str()); - } static void simulationinterfaces_DeleteEntity(const AZ::ConsoleCommandContainer& arguments) @@ -166,11 +156,10 @@ namespace SimulationInterfacesCommands } else { - AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().m_errorString.c_str()); } }; SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, cb); - } static void simulationinterfaces_GetSpawnables(const AZ::ConsoleCommandContainer& arguments) @@ -180,7 +169,7 @@ namespace SimulationInterfacesCommands SimulationEntityManagerRequestBus::BroadcastResult(spawnables, &SimulationEntityManagerRequestBus::Events::GetSpawnables); if (!spawnables.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().m_errorString.c_str()); return; } for (const auto& spawnable : spawnables.GetValue()) @@ -205,24 +194,23 @@ namespace SimulationInterfacesCommands AZ::Transform initialPose = AZ::Transform::CreateIdentity(); if (arguments.size() > 5) { - initialPose.SetTranslation( - AZ::Vector3( - AZStd::stof(AZStd::string(arguments[3])), - AZStd::stof(AZStd::string(arguments[4])), - AZStd::stof(AZStd::string(arguments[5])))); + initialPose.SetTranslation(AZ::Vector3( + AZStd::stof(AZStd::string(arguments[3])), + AZStd::stof(AZStd::string(arguments[4])), + AZStd::stof(AZStd::string(arguments[5])))); } SpawnCompletedCb completedCb = [](const AZ::Outcome& result) { if (!result.IsSuccess()) { - AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().error_string.c_str()); + AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().m_errorString.c_str()); return; } AZ_Printf("SimulationInterfacesConsole", "Entity spawned and registered : %s\n", result.GetValue().c_str()); - }; constexpr bool allowRename = true; - SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb); + SimulationEntityManagerRequestBus::Broadcast( + &SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb); AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); } @@ -245,7 +233,8 @@ namespace SimulationInterfacesCommands } else { - AZ_Printf("SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().error_string.c_str()); + AZ_Printf( + "SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().m_errorString.c_str()); } }; SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, cb); @@ -258,7 +247,6 @@ namespace SimulationInterfacesCommands AZ_CONSOLEFREEFUNC(simulationinterfaces_Resume, AZ::ConsoleFunctorFlags::DontReplicate, "Resume simulation."); AZ_CONSOLEFREEFUNC(simulationinterfaces_Step, AZ::ConsoleFunctorFlags::DontReplicate, "Step simulation."); - AZ_CONSOLEFREEFUNC( simulationinterfaces_GetEntitiesSphere, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the radius."); AZ_CONSOLEFREEFUNC(simulationinterfaces_GetEntityState, AZ::ConsoleFunctorFlags::DontReplicate, "Get state of the entity."); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 3cee2904b7..a6de9dfc2d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -28,7 +29,6 @@ #include #include #include -#include #include #include @@ -37,22 +37,34 @@ namespace SimulationInterfaces { - void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) + namespace { - if (!state.m_twist_angular.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state) { - // get transform - AZ::Vector3 angularVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_angular); - rigidBody->SetAngularVelocity(angularVelWorld); + if (!state.m_twistAngular.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + { + // get transform + AZ::Vector3 angularVelWorld = rigidBody->GetTransform().TransformVector(state.m_twistAngular); + rigidBody->SetAngularVelocity(angularVelWorld); + } + + if (!state.m_twistLinear.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + { + // get transform + AZ::Vector3 linearVelWorld = rigidBody->GetTransform().TransformVector(state.m_twistLinear); + rigidBody->SetLinearVelocity(linearVelWorld); + } } - if (!state.m_twist_linear.IsClose(AZ::Vector3::CreateZero(), AZ::Constants::FloatEpsilon)) + AzPhysics::Scene* GetSceneHelper(AzPhysics::SceneHandle sceneHandle) { - // get transform - AZ::Vector3 linearVelWorld = rigidBody->GetTransform().TransformVector(state.m_twist_linear); - rigidBody->SetLinearVelocity(linearVelWorld); + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "Physics system is not available."); + AzPhysics::Scene* scene = physicsSystem->GetScene(sceneHandle); + return scene; } - } + + } // namespace AZ_COMPONENT_IMPL(SimulationEntitiesManager, "SimulationEntitiesManager", SimulationEntitiesManagerTypeId); @@ -106,15 +118,6 @@ namespace SimulationInterfaces { } - AzPhysics::Scene* GetSceneHelper(AzPhysics::SceneHandle sceneHandle) - { - AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); - AZ_Assert(physicsSystem, "Physics system is not available."); - AzPhysics::Scene* scene = physicsSystem->GetScene(sceneHandle); - return scene; - } - - bool SimulationEntitiesManager::RegisterNewSimulatedBody(AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) { auto* scene = GetSceneHelper(sceneHandle); @@ -158,8 +161,7 @@ namespace SimulationInterfaces auto spawnDataIt = m_spawnCompletedCallbacks.find(ticketId); const bool wasSpawned = spawnDataIt != m_spawnCompletedCallbacks.end(); - const AZStd::string proposedName = - wasSpawned ? spawnDataIt->second.m_userProposedName : entity->GetName(); + const AZStd::string proposedName = wasSpawned ? spawnDataIt->second.m_userProposedName : entity->GetName(); // register entity const AZStd::string registeredName = this->AddSimulatedEntity(entityId, proposedName); @@ -176,8 +178,8 @@ namespace SimulationInterfaces initialState.m_pose = entity->GetTransform()->GetWorldTM(); if (rigidBody) { - initialState.m_twist_linear = rigidBody->GetLinearVelocity(); - initialState.m_twist_angular = rigidBody->GetAngularVelocity(); + initialState.m_twistLinear = rigidBody->GetLinearVelocity(); + initialState.m_twistAngular = rigidBody->GetAngularVelocity(); } m_entityIdToInitialState[entityId] = initialState; AZ_Info("SimulationInterfaces", "Registered entity %s\n", registeredName.c_str()); @@ -224,7 +226,10 @@ namespace SimulationInterfaces m_sceneAddedHandler = AzPhysics::SystemEvents::OnSceneAddedEvent::Handler( [this](AzPhysics::SceneHandle sceneHandle) { - AZ_Warning("SimulationInterfaces", m_physicsScenesHandle == AzPhysics::InvalidSceneHandle, "Hmm, we already have a scene"); + AZ_Warning( + "SimulationInterfaces", + m_physicsScenesHandle == AzPhysics::InvalidSceneHandle, + "SimulationInterfaces already gathered physics scene"); auto* scene = GetSceneHelper(sceneHandle); AZ_Assert(scene, "Scene is not available."); if (scene == nullptr) @@ -259,15 +264,15 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ // simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, - // simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, - // simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, - simulation_interfaces::msg::SimulatorFeatures::DELETING, - simulation_interfaces::msg::SimulatorFeatures::SPAWNABLES, - simulation_interfaces::msg::SimulatorFeatures::SPAWNING }); + AZStd::unordered_set{ // simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, + // simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, + // simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING, + simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, + simulation_interfaces::msg::SimulatorFeatures::DELETING, + simulation_interfaces::msg::SimulatorFeatures::SPAWNABLES, + simulation_interfaces::msg::SimulatorFeatures::SPAWNING }); AZ::TickBus::Handler::BusConnect(); } @@ -275,28 +280,18 @@ namespace SimulationInterfaces { AZ::TickBus::Handler::BusDisconnect(); SimulationEntityManagerRequestBus::Handler::BusDisconnect(); - if (m_simulationBodyAddedHandler.IsConnected()) - { - m_simulationBodyAddedHandler.Disconnect(); - } - if (m_simulationBodyRemovedHandler.IsConnected()) - { - m_simulationBodyRemovedHandler.Disconnect(); - } + + m_simulationBodyAddedHandler.Disconnect(); + m_simulationBodyRemovedHandler.Disconnect(); + m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; - if (m_sceneAddedHandler.IsConnected()) - { - m_sceneAddedHandler.Disconnect(); - } - if (m_sceneAddedHandler.IsConnected()) - { - m_sceneAddedHandler.Disconnect(); - } + + m_sceneAddedHandler.Disconnect(); + m_sceneAddedHandler.Disconnect(); } AZStd::string SimulationEntitiesManager::AddSimulatedEntity(AZ::EntityId entityId, const AZStd::string& userProposedName) { - if (!entityId.IsValid()) { return ""; @@ -332,15 +327,15 @@ namespace SimulationInterfaces AZ::Outcome SimulationEntitiesManager::GetEntities(const EntityFilters& filter) { - if (!filter.m_tags_filter.m_tags.empty()) + if (!filter.m_tagsFilter.m_tags.empty()) { AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); return AZ::Failure( FailedResult(simulation_interfaces::msg::Result::RESULT_FEATURE_UNSUPPORTED, "Tags filter is not implemented yet")); } - const bool reFilter = !filter.m_filter.empty(); - const bool shapeCastFilter = filter.m_bounds_shape != nullptr; + const bool reFilter = !filter.m_nameFilter.empty(); + const bool shapeCastFilter = filter.m_boundsShape != nullptr; AZStd::vector entities; if (!shapeCastFilter) @@ -368,8 +363,8 @@ namespace SimulationInterfaces } AzPhysics::OverlapRequest request; - request.m_shapeConfiguration = filter.m_bounds_shape; - request.m_pose = filter.m_bounds_pose; + request.m_shapeConfiguration = filter.m_boundsShape; + request.m_pose = filter.m_boundsPose; request.m_maxResults = AZStd::numeric_limits::max(); AzPhysics::SceneQueryHits result = sceneInterface->QueryScene(m_physicsScenesHandle, &request); @@ -387,7 +382,7 @@ namespace SimulationInterfaces { const AZStd::vector prefilteredEntities = AZStd::move(entities); entities.clear(); - const AZStd::regex regex(filter.m_filter); + const AZStd::regex regex(filter.m_nameFilter); if (!regex.Valid()) { AZ_Warning("SimulationInterfaces", false, "Invalid regex filter"); @@ -426,14 +421,14 @@ namespace SimulationInterfaces // transform linear and angular velocities to entity frame const AZ::Transform entityTransformInv = entityState.m_pose.GetInverse(); - entityState.m_twist_linear = entityTransformInv.TransformVector(linearVelocity); - entityState.m_twist_angular = entityTransformInv.TransformVector(angularVelocity); + entityState.m_twistLinear = entityTransformInv.TransformVector(linearVelocity); + entityState.m_twistAngular = entityTransformInv.TransformVector(angularVelocity); return AZ::Success(entityState); } AZ::Outcome SimulationEntitiesManager::GetEntitiesStates(const EntityFilters& filter) { - if (!filter.m_tags_filter.m_tags.empty()) + if (!filter.m_tagsFilter.m_tags.empty()) { AZ_Warning("SimulationInterfaces", false, "Tags filter is not implemented yet"); return AZ::Failure( @@ -514,7 +509,7 @@ namespace SimulationInterfaces } } - if (!state.m_twist_linear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twist_angular.IsZero(AZ::Constants::FloatEpsilon)) + if (!state.m_twistLinear.IsZero(AZ::Constants::FloatEpsilon) || !state.m_twistAngular.IsZero(AZ::Constants::FloatEpsilon)) { // get rigid body AzPhysics::RigidBody* rigidBody = nullptr; @@ -571,23 +566,6 @@ namespace SimulationInterfaces const auto msg = AZStd::string::format("Entity %s was not spawned by this component, wont delete it", name.c_str()); completedCb(AZ::Failure(FailedResult(simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED, msg))); } -#ifdef POTENTIALY_UNSAFE - if (findIt != m_simulatedEntityToEntityIdMap.end()) - { - const AZ::EntityId entityId = findIt->second; - AZ_Assert(entityId.IsValid(), "EntityId is not valid"); - // get all descendants - AZStd::vector entityAndDescendants; - AZ::TransformBus::EventResult(entityAndDescendants, entityId, &AZ::TransformBus::Events::GetEntityAndAllDescendants); - for (const auto& descendant : entityAndDescendants) - { - // I am not sure if this is the safe way to delete an entity - AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationRequests::DeleteEntity, descendant); - } - - return true; - } -#endif } void SimulationEntitiesManager::DeleteAllEntities(DeletionCompletedCb completedCb) @@ -790,7 +768,7 @@ namespace SimulationInterfaces } // check if name is still not unique, if not, add EntityId to name - if(m_simulatedEntityToEntityIdMap.contains(newName)) + if (m_simulatedEntityToEntityIdMap.contains(newName)) { newName = AZStd::string::format("%s_%s", newName.c_str(), entityId.ToString().c_str()); } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index 244033f2dd..7322467ae8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -35,8 +35,12 @@ namespace SimulationInterfaces SimulationEntitiesManager(); ~SimulationEntitiesManager(); + // AZ::Component interface implementation + void Init() override; + void Activate() override; + void Deactivate() override; - protected: + private: // SimulationEntityManagerRequestBus interface implementation AZ::Outcome GetEntities(const EntityFilters& filter) override; AZ::Outcome GetEntityState(const AZStd::string& name) override; @@ -54,16 +58,9 @@ namespace SimulationInterfaces SpawnCompletedCb completedCb) override; void ResetAllEntitiesToInitialState() override; - // AZ::Component interface implementation - void Init() override; - void Activate() override; - void Deactivate() override; - // AZ::TickBus::Handler interface implementation void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; - private: - //! Registers a new simulated body to the simulation interface. //! Note that the body handle will be registered under unique name //! Note that body need to be configured to be registered @@ -73,7 +70,8 @@ namespace SimulationInterfaces //! Registers a new simulated body to the simulation interface. //! Returns the list of handles that were not registered - AZStd::vector> RegisterNewSimulatedBodies(const AZStd::vector>& handles); + AZStd::vector> RegisterNewSimulatedBodies( + const AZStd::vector>& handles); //! Registers simulated entity to entity id mapping. //! Note that the entityId will be registered under unique name. @@ -91,7 +89,6 @@ namespace SimulationInterfaces //! Set the state of the entity and their descendants. void SetEntitiesState(const AZStd::vector& entityAndDescendants, const EntityState& state); - AzPhysics::SceneEvents::OnSimulationBodyAdded::Handler m_simulationBodyAddedHandler; AzPhysics::SceneEvents::OnSimulationBodyRemoved::Handler m_simulationBodyRemovedHandler; @@ -99,7 +96,8 @@ namespace SimulationInterfaces AzPhysics::SystemEvents::OnSceneRemovedEvent::Handler m_sceneRemovedHandler; AzPhysics::SceneHandle m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; - AZStd::vector> m_unconfiguredScenesHandles; //! Set of yet-invalid scenes handles, that are waiting for configuration + AZStd::vector> + m_unconfiguredScenesHandles; //! Set of yet-invalid scenes handles, that are waiting for configuration AZStd::unordered_map m_simulatedEntityToEntityIdMap; AZStd::unordered_map m_entityIdToSimulatedEntityMap; AZStd::unordered_map m_entityIdToInitialState; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp index f73760c13e..699779c1ac 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp @@ -74,17 +74,17 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Handler::BusDisconnect(); } - void SimulationFeaturesAggregator::AddSimulationFeatures(const AZStd::unordered_set& features) + void SimulationFeaturesAggregator::AddSimulationFeatures(const AZStd::unordered_set& features) { m_registeredFeatures.insert(features.begin(), features.end()); } - - const AZStd::unordered_set SimulationFeaturesAggregator::GetSimulationFeatures() const + + AZStd::unordered_set SimulationFeaturesAggregator::GetSimulationFeatures() { return m_registeredFeatures; } - bool SimulationFeaturesAggregator::HasFeature(SimulationFeatures feature) const + bool SimulationFeaturesAggregator::HasFeature(SimulationFeatureType feature) { return m_registeredFeatures.contains(feature); } diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h index 8fa8abe76c..9f445a1d48 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h @@ -42,13 +42,12 @@ namespace SimulationInterfaces void Activate() override; void Deactivate() override; - protected: + private: // SimulationFeaturesAggregatorRequestBus overrides - void AddSimulationFeatures(const AZStd::unordered_set& features) override; - const AZStd::unordered_set GetSimulationFeatures() const override; - bool HasFeature(SimulationFeatures feature) const override; + void AddSimulationFeatures(const AZStd::unordered_set& features) override; + AZStd::unordered_set GetSimulationFeatures() override; + bool HasFeature(SimulationFeatureType feature) override; - private: - AZStd::unordered_set m_registeredFeatures; + AZStd::unordered_set m_registeredFeatures; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index fa3adb5750..2c941584c5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -105,16 +105,16 @@ namespace SimulationInterfaces SimulationManagerRequestBus::Handler::BusConnect(); SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_TIME, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_STATE, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_SPAWNED, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_PAUSE, - simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_SINGLE, - simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_MULTIPLE, - simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_ACTION, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_SETTING, - simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_GETTING }); + AZStd::unordered_set{ simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_TIME, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_STATE, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_RESET_SPAWNED, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_PAUSE, + simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_SINGLE, + simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_MULTIPLE, + simulation_interfaces::msg::SimulatorFeatures::STEP_SIMULATION_ACTION, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_SETTING, + simulation_interfaces::msg::SimulatorFeatures::SIMULATION_STATE_GETTING }); AZ::SystemTickBus::QueueFunction( [this]() { diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index bc8f85b5af..7db04c7704 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -46,7 +46,7 @@ namespace SimulationInterfaces void Activate() override; void Deactivate() override; - protected: + private: // SimulationManagerRequestBus interface implementation void SetSimulationPaused(bool paused) override; void StepSimulation(AZ::u64 steps) override; @@ -60,7 +60,6 @@ namespace SimulationInterfaces // LevelSystemLifecycleNotificationBus interface implementation void OnLoadingComplete(const char* levelName) override; - private: bool m_isSimulationPaused = false; uint64_t m_numberOfPhysicsSteps = 0; AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_simulationFinishEvent; @@ -70,7 +69,6 @@ namespace SimulationInterfaces }; // default simulation state based on standard void InitializeSimulationState(); - private: bool IsTransitionForbidden(SimulationState requestedState); // forbidden transition between state, first is current state, second is desire state const AZStd::array, 4> m_forbiddenStatesTransitions{ { diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp index 22812e11ca..c5b31a09c4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -36,8 +36,8 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); }); diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp index 66f2d41539..ead483edc7 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -43,8 +43,8 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index 9c3842ea24..77cee8486a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -45,40 +45,28 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } const auto& multipleEntitiesStates = outcome.GetValue(); - std::vector stdEntities; - std::vector stdEntityStates; - AZStd::transform( - multipleEntitiesStates.begin(), - multipleEntitiesStates.end(), - std::back_inserter(stdEntities), - [](const auto& pair) - { - return pair.first.c_str(); - }); - AZStd::transform( - multipleEntitiesStates.begin(), - multipleEntitiesStates.end(), - std::back_inserter(stdEntityStates), - [](const auto& pair) - { - const SimulationInterfaces::EntityState& entityState = pair.second; - simulation_interfaces::msg::EntityState simulationInterfacesEntityState; - simulationInterfacesEntityState.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); - simulationInterfacesEntityState.header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); - simulationInterfacesEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); - simulationInterfacesEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); - simulationInterfacesEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); - return simulationInterfacesEntityState; - }); - response.entities = stdEntities; - response.states = stdEntityStates; + response.entities.reserve(multipleEntitiesStates.size()); + response.states.reserve(multipleEntitiesStates.size()); + for (auto& [entityName, entityState] : multipleEntitiesStates) + { + // entity name + response.entities.push_back(entityName.c_str()); + // entity state + simulation_interfaces::msg::EntityState simulationInterfacesEntityState; + simulationInterfacesEntityState.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + simulationInterfacesEntityState.header.frame_id = ""; + simulationInterfacesEntityState.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); + simulationInterfacesEntityState.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistLinear); + simulationInterfacesEntityState.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistAngular); + response.states.push_back(simulationInterfacesEntityState); + } return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp index 00b50f013f..0bf0588d8a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -33,18 +33,18 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } const auto& entityState = outcome.GetValue(); simulation_interfaces::msg::EntityState entityStateMsg; entityStateMsg.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); - entityStateMsg.header.frame_id = ROS2::ROS2Interface::Get()->GetNode()->get_name(); + entityStateMsg.header.frame_id = ""; entityStateMsg.pose = ROS2::ROS2Conversions::ToROS2Pose(entityState.m_pose); - entityStateMsg.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_linear); - entityStateMsg.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twist_angular); + entityStateMsg.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistLinear); + entityStateMsg.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(entityState.m_twistAngular); response.result.result = simulation_interfaces::msg::Result::RESULT_OK; response.state = entityStateMsg; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 5a4aea23d6..658150ebe5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -10,37 +10,36 @@ #include #include #include -#include namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() + AZStd::unordered_set GetSimulationFeaturesServiceHandler::GetProvidedFeatures() { // standard doesn't define specific feature id for this service - return AZStd::unordered_set{}; + return AZStd::unordered_set{}; } AZStd::optional GetSimulationFeaturesServiceHandler::HandleServiceRequest( const std::shared_ptr header, const Request& request) { // call bus to get simulation features in ROS2SimulationInterfaces Gem side - AZStd::unordered_set ros2Interfaces; + AZStd::unordered_set ros2Interfaces; ROS2SimulationInterfacesRequestBus::BroadcastResult(ros2Interfaces, &ROS2SimulationInterfacesRequests::GetSimulationFeatures); // call bus to get simulation features on SimulationInterfaces Gem side - AZStd::unordered_set o3deInterfaces; + AZStd::unordered_set o3deInterfaces; SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::BroadcastResult( o3deInterfaces, &SimulationInterfaces::SimulationFeaturesAggregatorRequests::GetSimulationFeatures); // create common features and return it; // common features are logical AND between two sets - AZStd::unordered_set commonFeatures; + AZStd::unordered_set commonFeatures; commonFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); commonFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end()); Response response; for (auto id : commonFeatures) { - if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatures(id))) + if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatureType(id))) { response.features.features.emplace_back(id); } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h index 568ade2e66..8e993943b4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -10,6 +10,7 @@ #include "ROS2ServiceBase.h" #include +#include #include namespace ROS2SimulationInterfaces @@ -27,7 +28,7 @@ namespace ROS2SimulationInterfaces { return "get_simulation_features"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h index 2ca83acdbc..a7cb5e4ebc 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h @@ -10,7 +10,6 @@ #include "Services/ROS2ServiceBase.h" #include -#include #include namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp index c2fa782ba3..2baba4a642 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -28,17 +28,16 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); return response; } const auto& spawnableList = outcome.GetValue(); - std::vector simSpawnables; AZStd::transform( spawnableList.begin(), spawnableList.end(), - AZStd::back_inserter(simSpawnables), + AZStd::back_inserter(response.spawnables), [](const SimulationInterfaces::Spawnable& spawnable) { simulation_interfaces::msg::Spawnable simSpawnable; @@ -47,7 +46,6 @@ namespace ROS2SimulationInterfaces return simSpawnable; }); response.result.result = simulation_interfaces::msg::Result::RESULT_OK; - response.spawnables = simSpawnables; return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h index 6e095a2958..b53f46c748 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h @@ -12,7 +12,6 @@ #include #include #include -#include #include #include diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index 5a2ff4e0c9..e778126a97 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -52,8 +52,8 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); }; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h index 1bb965fd25..b98995c4bd 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h @@ -10,7 +10,6 @@ #include "ROS2ServiceBase.h" #include -#include #include namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp index 303b566c67..2064c5b92c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -25,8 +25,8 @@ namespace ROS2SimulationInterfaces AZStd::string entityName = request.entity.c_str(); SimulationInterfaces::EntityState entityState; entityState.m_pose = ROS2::ROS2Conversions::FromROS2Pose(request.state.pose); - entityState.m_twist_angular = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.angular); - entityState.m_twist_linear = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.linear); + entityState.m_twistAngular = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.angular); + entityState.m_twistLinear = ROS2::ROS2Conversions::FromROS2Vector3(request.state.twist.linear); SimulationInterfaces::SimulationEntityManagerRequestBus::BroadcastResult( outcome, &SimulationInterfaces::SimulationEntityManagerRequests::SetEntityState, entityName, entityState); @@ -36,8 +36,8 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } return response; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp index b1642b32ff..9461c236da 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -33,8 +33,8 @@ namespace ROS2SimulationInterfaces } else { - response.result.result = static_cast(transitionResult.GetError().error_code); - response.result.error_message = transitionResult.GetError().error_string.c_str(); + response.result.result = static_cast(transitionResult.GetError().m_errorCode); + response.result.error_message = transitionResult.GetError().m_errorString.c_str(); } return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h index 9222be60e7..c63bba3efd 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h @@ -10,7 +10,6 @@ #include "Services/ROS2ServiceBase.h" #include -#include #include namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp index df46d68550..b714eceb81 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -28,7 +28,7 @@ namespace ROS2SimulationInterfaces const AZStd::string_view entityNamespace{ request.entity_namespace.c_str(), request.entity_namespace.size() }; // Validate entity name - if (!ValidateEntityName(name)) + if (!name.empty() && !ValidateEntityName(name)) { Response response; response.result.result = simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID; @@ -42,7 +42,8 @@ namespace ROS2SimulationInterfaces { Response response; response.result.result = simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID; - response.result.error_message = "Invalid entity namespace. Entity namespaces can only contain alphanumeric characters and forward slashes."; + response.result.error_message = + "Invalid entity namespace. Entity namespaces can only contain alphanumeric characters and forward slashes."; SendResponse(response); return AZStd::nullopt; } @@ -66,8 +67,8 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.error_code); - response.result.error_message = failedResult.error_string.c_str(); + response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); }); @@ -82,7 +83,9 @@ namespace ROS2SimulationInterfaces bool SpawnEntityServiceHandler::ValidateFrameName(const AZStd::string& frameName) { - const AZStd::regex frameRegex{ R"(^[a-zA-Z0-9_/]+$)" }; // Entity names can only contain alphanumeric characters and underscores and forward slashes + const AZStd::regex frameRegex{ + R"(^[a-zA-Z0-9_/]+$)" + }; // Entity names can only contain alphanumeric characters and underscores and forward slashes return AZStd::regex_match(frameName, frameRegex); } diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h index e04413edd0..57d7f2f8a8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h @@ -19,7 +19,7 @@ namespace ROS2SimulationInterfaces::Utils AZ::Outcome GetEntityFiltersFromRequest(const RequestType& request) { SimulationInterfaces::EntityFilters filter; - filter.m_filter = request.filters.filter.c_str(); + filter.m_nameFilter = request.filters.filter.c_str(); uint8_t type = request.filters.bounds.type; if (type == simulation_interfaces::msg::Bounds::TYPE_BOX) { @@ -27,8 +27,8 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_BOX' should have exactly 2 points."); } - const auto &p1 = request.filters.bounds.points.front(); - const auto &p2 = request.filters.bounds.points.back(); + const auto& p1 = request.filters.bounds.points.front(); + const auto& p2 = request.filters.bounds.points.back(); if (p1.x > p2.x || p1.y > p2.y || p1.z > p2.z) { return AZ::Failure("Invalid points! The first point should be lower than the second point."); @@ -36,7 +36,7 @@ namespace ROS2SimulationInterfaces::Utils const auto upperRight = ROS2::ROS2Conversions::FromROS2Vector3(p2); const auto lowerLeft = ROS2::ROS2Conversions::FromROS2Vector3(p1); const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(lowerLeft, upperRight); - filter.m_bounds_shape = AZStd::make_shared(aabb.GetExtents()); + filter.m_boundsShape = AZStd::make_shared(aabb.GetExtents()); } else if (type == simulation_interfaces::msg::Bounds::TYPE_CONVEX_HULL) // TYPE_CONVEX_HULL { @@ -44,7 +44,7 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have at least 3 points."); } - filter.m_bounds_shape = AZStd::make_shared(); + filter.m_boundsShape = AZStd::make_shared(); } else if (type == simulation_interfaces::msg::Bounds::TYPE_SPHERE) // TYPE_SPHERE { @@ -52,10 +52,10 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_SPHERE' should have exactly 2 points."); } - filter.m_bounds_shape = AZStd::make_shared(request.filters.bounds.points.back().x); - filter.m_bounds_pose = { ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()), - AZ::Quaternion::CreateIdentity(), - 1.0f }; + filter.m_boundsShape = AZStd::make_shared(request.filters.bounds.points.back().x); + filter.m_boundsPose = { ROS2::ROS2Conversions::FromROS2Vector3(request.filters.bounds.points.front()), + AZ::Quaternion::CreateIdentity(), + 1.0f }; } return AZ::Success(AZStd::move(filter)); } diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp index 8708bc73f7..71a2b52c54 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include -#include namespace UnitTest { @@ -146,7 +146,6 @@ namespace UnitTest EXPECT_NE(services.find("/spawn_entity"), services.end()); EXPECT_NE(services.find("/get_simulation_features"), services.end()); EXPECT_NE(services.find("/step_simulation"), services.end()); - } //! Test if the service call succeeds when the entity is found @@ -195,8 +194,8 @@ namespace UnitTest { EXPECT_EQ(name, "test_entity"); FailedResult failedResult; - failedResult.error_code = simulation_interfaces::msg::Result::RESULT_NOT_FOUND, - failedResult.error_string = "FooBar not found."; + failedResult.m_errorCode = simulation_interfaces::msg::Result::RESULT_NOT_FOUND, + failedResult.m_errorString = "FooBar not found."; completedCb(AZ::Failure(failedResult)); })); @@ -230,21 +229,21 @@ namespace UnitTest .WillOnce(::testing::Invoke( [=](const EntityFilters& filter) { - EXPECT_NE(filter.m_bounds_shape, nullptr); - if (filter.m_bounds_shape) + EXPECT_NE(filter.m_boundsShape, nullptr); + if (filter.m_boundsShape) { - EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Sphere); + EXPECT_EQ(filter.m_boundsShape->GetShapeType(), Physics::ShapeType::Sphere); Physics::SphereShapeConfiguration* sphereShape = - azdynamic_cast(filter.m_bounds_shape.get()); + azdynamic_cast(filter.m_boundsShape.get()); EXPECT_EQ(sphereShape->m_radius, 99.0f); EXPECT_EQ(sphereShape->m_scale, AZ::Vector3(1.0f)); } - auto loc = filter.m_bounds_pose.GetTranslation(); + auto loc = filter.m_boundsPose.GetTranslation(); EXPECT_EQ(loc.GetX(), point1.GetX()); EXPECT_EQ(loc.GetY(), point1.GetY()); EXPECT_EQ(loc.GetZ(), point1.GetZ()); - EXPECT_EQ(filter.m_filter, "FooBarFilter"); + EXPECT_EQ(filter.m_nameFilter, "FooBarFilter"); return AZ::Success(EntityNameList{ "FooBar" }); })); @@ -300,17 +299,17 @@ namespace UnitTest .WillOnce(::testing::Invoke( [=](const EntityFilters& filter) { - EXPECT_NE(filter.m_bounds_shape, nullptr); - if (filter.m_bounds_shape) + EXPECT_NE(filter.m_boundsShape, nullptr); + if (filter.m_boundsShape) { - EXPECT_EQ(filter.m_bounds_shape->GetShapeType(), Physics::ShapeType::Box); + EXPECT_EQ(filter.m_boundsShape->GetShapeType(), Physics::ShapeType::Box); Physics::BoxShapeConfiguration* boxShape = - azdynamic_cast(filter.m_bounds_shape.get()); + azdynamic_cast(filter.m_boundsShape.get()); EXPECT_EQ(boxShape->m_dimensions.GetX(), dims.GetX()); EXPECT_EQ(boxShape->m_dimensions.GetY(), dims.GetY()); EXPECT_EQ(boxShape->m_dimensions.GetZ(), dims.GetZ()); } - auto loc = filter.m_bounds_pose.GetTranslation(); + auto loc = filter.m_boundsPose.GetTranslation(); EXPECT_EQ(loc, AZ::Vector3::CreateZero()); return AZ::Success(EntityNameList{ "FooBar" }); })); @@ -374,10 +373,10 @@ namespace UnitTest SimulationFeaturesAggregatorRequestsMockedHandler mockAggregator; using ::testing::_; auto node = GetRos2Node(); - AZStd::unordered_set features{ + AZStd::unordered_set features{ simulation_interfaces::msg::SimulatorFeatures::SPAWNING, - static_cast(0xFE), // invalid feature, should be ignored - static_cast(0xFF), // invalid feature, should be ignored + static_cast(0xFE), // invalid feature, should be ignored + static_cast(0xFF), // invalid feature, should be ignored }; EXPECT_CALL(mockAggregator, GetSimulationFeatures()) .WillOnce(::testing::Invoke( @@ -412,17 +411,21 @@ namespace UnitTest request->allow_renaming = true; auto future = client->async_send_request(request); - EXPECT_CALL(mock, SpawnEntity(_, _, _, _, _, _)).WillOnce( - ::testing::Invoke( - [](const AZStd::string& name, const AZStd::string& uri, const AZStd::string& entityNamespace, const AZ::Transform& initialPose, const bool allowRename, SpawnCompletedCb completedCb) + EXPECT_CALL(mock, SpawnEntity(_, _, _, _, _, _)) + .WillOnce(::testing::Invoke( + [](const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb) { EXPECT_EQ(name, "valid_name"); EXPECT_EQ(uri, "test_uri"); EXPECT_EQ(entityNamespace, "test_namespace"); EXPECT_TRUE(allowRename); completedCb(AZ::Success("valid_name")); - }) - ); + })); SpinAppSome(); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; @@ -448,8 +451,8 @@ namespace UnitTest ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); - EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID) << "Service call should fail with invalid name."; - + EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAME_INVALID) + << "Service call should fail with invalid name."; } //! Check if service fails when the namespace is invalid @@ -470,7 +473,8 @@ namespace UnitTest ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); - EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID) << "Service call should fail with invalid name."; + EXPECT_EQ(response->result.result, simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID) + << "Service call should fail with invalid name."; } TEST_F(SimulationInterfaceROS2TestFixture, SimulationStepsSuccess) diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h index 94c8f263dc..7c7f867d55 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h @@ -17,8 +17,8 @@ namespace UnitTest SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::Handler::BusDisconnect(); } - MOCK_METHOD(void, AddSimulationFeatures, (const AZStd::unordered_set& features), (override)); - MOCK_METHOD(const AZStd::unordered_set, GetSimulationFeatures, (), (const, override)); - MOCK_METHOD(bool, HasFeature, (SimulationFeatures feature), (const, override)); + MOCK_METHOD(void, AddSimulationFeatures, (const AZStd::unordered_set& features), (override)); + MOCK_METHOD(AZStd::unordered_set, GetSimulationFeatures, (), (override)); + MOCK_METHOD(bool, HasFeature, (SimulationFeatureType feature), (override)); }; } // namespace UnitTest diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp index 84b5096c66..50c07eb88f 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationInterfaceTests.cpp @@ -79,7 +79,7 @@ namespace UnitTest CreateEntityWithStaticBodyComponent("Outside", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); EntityFilters filter; - filter.m_bounds_shape = AZStd::make_shared(2.0f); + filter.m_boundsShape = AZStd::make_shared(2.0f); AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -103,7 +103,7 @@ namespace UnitTest CreateEntityWithStaticBodyComponent("WontMatch", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); EntityFilters filter; - filter.m_filter = "Will.*"; + filter.m_nameFilter = "Will.*"; AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -126,7 +126,7 @@ namespace UnitTest CreateEntityWithStaticBodyComponent("WontMatch", AZ::Transform::CreateTranslation(AZ::Vector3(10.0f, 0.0f, 0.0f))); EntityFilters filter; - filter.m_filter = "[a-z"; + filter.m_nameFilter = "[a-z"; AZ::Outcome entities; SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -168,7 +168,7 @@ namespace UnitTest EXPECT_GT(deltaPos.GetLength(), 0.0f); // check if entity has velocity - EXPECT_GT(stateAfter.m_twist_linear.GetLength(), 0.0f); + EXPECT_GT(stateAfter.m_twistLinear.GetLength(), 0.0f); DeleteEntity(entityId1); } @@ -182,8 +182,7 @@ namespace UnitTest deletionWasCompleted = true; EXPECT_TRUE(result.IsSuccess()); }; - SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); TickApp(100); EXPECT_TRUE(deletionWasCompleted); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp index 50058b99fe..28233734e4 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp @@ -78,7 +78,13 @@ namespace UnitTest constexpr bool allowRename = false; SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, completedCb); + &SimulationEntityManagerRequestBus::Events::SpawnEntity, + entityName, + uri, + entityNamespace, + initialPose, + allowRename, + completedCb); // entities are spawned asynchronously, so we need to tick the app to let the entity be spawned TickApp(100); @@ -92,7 +98,13 @@ namespace UnitTest completed2 = true; }; SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, entityName, uri, entityNamespace, initialPose, allowRename, failedSpawnCompletedCb); + &SimulationEntityManagerRequestBus::Events::SpawnEntity, + entityName, + uri, + entityNamespace, + initialPose, + allowRename, + failedSpawnCompletedCb); EXPECT_TRUE(completed2); // list simulation entities @@ -136,8 +148,8 @@ namespace UnitTest // Check if entity was teleported by setting the new state, we use a filter to check if the entity is at the new position EntityFilters filter; - filter.m_bounds_shape = AZStd::make_shared(2.0f); - filter.m_bounds_pose = AZ::Transform::CreateTranslation(AZ::Vector3(1000.0f, 0.0f, 0.0f)); + filter.m_boundsShape = AZStd::make_shared(2.0f); + filter.m_boundsPose = AZ::Transform::CreateTranslation(AZ::Vector3(1000.0f, 0.0f, 0.0f)); AZ::Outcome entitiesFiltered; SimulationEntityManagerRequestBus::BroadcastResult( entitiesFiltered, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); @@ -157,7 +169,9 @@ namespace UnitTest EXPECT_EQ(getNumberOfEntities(), 0); // spawn 3 entities of entities and despawn all of them - SpawnCompletedCb cb = [&](const AZ::Outcome& result){}; + SpawnCompletedCb cb = [&](const AZ::Outcome& result) + { + }; SimulationEntityManagerRequestBus::Broadcast( &SimulationEntityManagerRequestBus::Events::SpawnEntity, "entity1", uri, entityNamespace, initialPose, false, cb); SimulationEntityManagerRequestBus::Broadcast( @@ -174,13 +188,11 @@ namespace UnitTest deletionWasCompleted = true; EXPECT_TRUE(result.IsSuccess()); }; - SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); + SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, deleteAllCompletion); TickApp(100); EXPECT_TRUE(deletionWasCompleted); EXPECT_EQ(getNumberOfEntities(), 0); - } } // namespace UnitTest From f0b2de705bb3474990331cd8cb043409e1e976d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 16 Apr 2025 16:38:50 +0200 Subject: [PATCH 29/35] Review comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- Gems/ROS2/Code/CMakeLists.txt | 7 +++++++ Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h | 3 +++ Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h | 4 +++- Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h | 3 +++ .../Source/ContactSensor/ROS2ContactSensorComponent.cpp | 4 +--- .../Code/Source/ContactSensor/ROS2ContactSensorComponent.h | 3 --- Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp | 4 +--- Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h | 4 +--- .../Source/Spawner/ROS2SpawnPointComponentController.cpp | 4 +--- .../Source/Spawner/ROS2SpawnPointComponentController.h | 4 +--- .../Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp | 4 +--- .../Code/Source/Spawner/ROS2SpawnPointEditorComponent.h | 4 +--- Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp | 4 +--- Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h | 4 +--- .../Code/Source/Spawner/ROS2SpawnerComponentController.cpp | 4 +--- .../Code/Source/Spawner/ROS2SpawnerComponentController.h | 4 +--- .../Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp | 4 +--- Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h | 4 +--- .../SimulationFeaturesAggregatorRequestBus.h | 3 ++- .../SimulationInterfaces/SimulationMangerRequestBus.h | 3 ++- .../Code/Source/Clients/SimulationEntitiesManager.cpp | 2 +- 21 files changed, 34 insertions(+), 46 deletions(-) diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index ac0e349d87..fe0f8f2b99 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -49,6 +49,13 @@ 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) diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h index ea1d1d486a..bab1099d22 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h @@ -34,6 +34,9 @@ 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. diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h index 4e66aaf37c..599a4ace3f 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h @@ -26,7 +26,9 @@ namespace ROS2 //! @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; }; diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h index 9006dd4658..6f958e7643 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h @@ -30,6 +30,9 @@ 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. diff --git a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp index 8c3ac813ac..8f0f8de88d 100644 --- a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2ContactSensorComponent.h" #include #include diff --git a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h index efba4ecb4e..9584b4bae6 100644 --- a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h +++ b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h @@ -7,9 +7,6 @@ */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif #include #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp index af2cfea763..a409add6b3 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnPointComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h index ba2be3cab4..da7ffa7b6a 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h @@ -6,9 +6,7 @@ * */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "Spawner/ROS2SpawnPointComponentController.h" #include #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp index 06fc6a788c..d0aa41491a 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "Spawner/ROS2SpawnPointComponentController.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h index 9a510d3001..de241cc93a 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h @@ -6,9 +6,7 @@ * */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include #include #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp index 7074bbb1fa..b1d31087e5 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2SpawnPointEditorComponent.h" #include "Spawner/ROS2SpawnPointComponentController.h" #include "Spawner/ROS2SpawnerEditorComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h index 2c35fd7935..56e79cb3d7 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h @@ -6,9 +6,7 @@ * */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "Spawner/ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnPointComponentController.h" #include "Spawner/ROS2SpawnerEditorComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp index 2498cc9a72..9e9a6a9314 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2SpawnerComponent.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h index 5f3482ed29..a9eb16aa65 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h @@ -6,9 +6,7 @@ * */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp index 210701514f..fdec3e1998 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2SpawnerComponentController.h" #include "Spawner/ROS2SpawnPointComponent.h" #include "Spawner/ROS2SpawnerComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h index 513cf7b6e8..19e07c596d 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h @@ -7,9 +7,7 @@ */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2/Spawner/SpawnerBus.h" #include "ROS2SpawnPointComponent.h" #include diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp index df33f1a6ec..dd667dda9f 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp @@ -5,9 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "ROS2SpawnerEditorComponent.h" #include "AzCore/Debug/Trace.h" #include "ROS2SpawnPointEditorComponent.h" diff --git a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h index 72e099667b..20cc92581e 100644 --- a/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h +++ b/Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h @@ -6,9 +6,7 @@ * */ #pragma once -#ifndef WITH_GAZEBO_MSGS -static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined."); -#endif + #include "Spawner/ROS2SpawnerComponent.h" #include "Spawner/ROS2SpawnerComponentController.h" #include diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h index 805a03b228..c961a5f3d8 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h @@ -13,10 +13,11 @@ #include #include #include +#include namespace SimulationInterfaces { - using SimulationFeatureType = uint8_t; + using SimulationFeatureType = simulation_interfaces::msg::SimulatorFeatures::_features_type::value_type; class SimulationFeaturesAggregatorRequests { diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 17f45935b8..2f683e1e64 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -15,10 +15,11 @@ #include #include #include +#include namespace SimulationInterfaces { - using SimulationState = uint8_t; + using SimulationState = simulation_interfaces::msg::SimulationState::_state_type; class SimulationManagerRequests { diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index a6de9dfc2d..fb349e7960 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -382,7 +382,7 @@ namespace SimulationInterfaces { const AZStd::vector prefilteredEntities = AZStd::move(entities); entities.clear(); - const AZStd::regex regex(filter.m_nameFilter); + const AZStd::regex regex(filter.m_nameFilter, AZStd::regex::extended); if (!regex.Valid()) { AZ_Warning("SimulationInterfaces", false, "Invalid regex filter"); From 466fcca8a259127f7096913cabb0a706d42ffca6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 16 Apr 2025 16:43:49 +0200 Subject: [PATCH 30/35] Clang format applied MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Clients/ROS2SimulationInterfacesSystemComponent.h | 2 +- .../Code/Source/Clients/SimulationEntitiesManager.cpp | 1 - .../Code/Source/Services/SpawnEntityServiceHandler.h | 4 +--- .../Source/Services/StepSimulationServiceHandler.cpp | 3 +-- .../Source/Services/StepSimulationServiceHandler.h | 2 +- .../Source/SimulationInterfacesModuleInterface.cpp | 2 +- .../Tests/Clients/ROS2SimulationInterfacesTest.cpp | 2 +- .../Tests/Tools/Mocks/SimulationEntityManagerMock.h | 11 ++++++++--- 8 files changed, 14 insertions(+), 13 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h index 64dae28a6c..8e4720b127 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h @@ -19,8 +19,8 @@ #include "Services/GetSimulationStateServiceHandler.h" #include "Services/GetSpawnablesServiceHandler.h" #include "Services/ROS2ServiceBase.h" -#include "Services/SetEntityStateServiceHandler.h" #include "Services/ResetSimulationServiceHandler.h" +#include "Services/SetEntityStateServiceHandler.h" #include "Services/SetSimulationStateServiceHandler.h" #include "Services/SpawnEntityServiceHandler.h" #include "Services/StepSimulationServiceHandler.h" diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index fb349e7960..0d886f7df1 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -487,7 +487,6 @@ namespace SimulationInterfaces for (const auto& descendant : entityAndDescendants) { AzPhysics::SimulatedBodyComponentRequestsBus::Event(descendant, &AzPhysics::SimulatedBodyComponentRequests::DisablePhysics); - } if (parentEntityId.IsValid()) { diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h index 8fa8678c33..f2021d02f2 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h @@ -9,9 +9,9 @@ #pragma once #include "ROS2ServiceBase.h" +#include #include #include -#include namespace ROS2SimulationInterfaces { class SpawnEntityServiceHandler : public ROS2ServiceBase @@ -33,8 +33,6 @@ namespace ROS2SimulationInterfaces private: bool ValidateEntityName(const AZStd::string& entityName); bool ValidateFrameName(const AZStd::string& frameName); - - }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp index ce6a9d6da4..0291fcbb5a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp @@ -16,8 +16,7 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set StepSimulationServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_SINGLE, - SimulationFeatures::STEP_SIMULATION_MULTIPLE }; + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_SINGLE, SimulationFeatures::STEP_SIMULATION_MULTIPLE }; } AZStd::optional StepSimulationServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h index a41d5649df..4899f27d48 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h @@ -9,9 +9,9 @@ #pragma once #include "ROS2ServiceBase.h" +#include #include #include -#include namespace ROS2SimulationInterfaces { diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp index e8262cf970..598d4a802b 100644 --- a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -13,8 +13,8 @@ #include "Clients/SimulationFeaturesAggregator.h" #include "Clients/SimulationManager.h" -#include #include +#include namespace SimulationInterfaces { diff --git a/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp index 980bd781d5..40217ff9bc 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp @@ -5,7 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ - + #include AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h index 11bf4264bd..336981c295 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/Mocks/SimulationEntityManagerMock.h @@ -27,7 +27,12 @@ namespace UnitTest 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)); - + void( + const AZStd::string& name, + const AZStd::string& uri, + const AZStd::string& entityNamespace, + const AZ::Transform& initialPose, + const bool allowRename, + SpawnCompletedCb completedCb)); }; -} +} // namespace UnitTest From 99aede3b0fcced53b645e08192fd72f39a6d27a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 19 Apr 2025 10:43:40 +0100 Subject: [PATCH 31/35] Alias type from ROS 2 messages, instead of hardcoding (#892) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../ROS2SimulationInterfacesRequestBus.h | 4 ++-- .../Code/Include/SimulationInterfaces/Result.h | 3 ++- .../Code/Source/Actions/ROS2ActionBase.h | 2 +- .../Actions/SimulateStepsActionServerHandler.cpp | 8 ++++---- .../Actions/SimulateStepsActionServerHandler.h | 2 +- .../ROS2SimulationInterfacesSystemComponent.cpp | 4 ++-- .../ROS2SimulationInterfacesSystemComponent.h | 2 +- .../Code/Source/Interfaces/IROS2HandlerBase.h | 4 +++- .../Source/Services/DeleteEntityServiceHandler.cpp | 4 ++-- .../Source/Services/DeleteEntityServiceHandler.h | 2 +- .../Source/Services/GetEntitiesServiceHandler.cpp | 10 +++++----- .../Code/Source/Services/GetEntitiesServiceHandler.h | 2 +- .../Services/GetEntitiesStatesServiceHandler.cpp | 12 ++++++------ .../Services/GetEntitiesStatesServiceHandler.h | 2 +- .../Source/Services/GetEntityStateServiceHandler.cpp | 4 ++-- .../Source/Services/GetEntityStateServiceHandler.h | 2 +- .../Services/GetSimulationStateServiceHandler.cpp | 4 ++-- .../Services/GetSimulationStateServiceHandler.h | 2 +- .../Source/Services/GetSpawnablesServiceHandler.cpp | 4 ++-- .../Source/Services/GetSpawnablesServiceHandler.h | 2 +- .../Code/Source/Services/ROS2ServiceBase.h | 2 +- .../Services/ResetSimulationServiceHandler.cpp | 10 +++++----- .../Source/Services/ResetSimulationServiceHandler.h | 2 +- .../Source/Services/SetEntityStateServiceHandler.cpp | 4 ++-- .../Source/Services/SetEntityStateServiceHandler.h | 2 +- .../Services/SetSimulationStateServiceHandler.cpp | 5 +++-- .../Services/SetSimulationStateServiceHandler.h | 2 +- .../Source/Services/SpawnEntityServiceHandler.cpp | 4 ++-- .../Code/Source/Services/SpawnEntityServiceHandler.h | 2 +- .../Source/Services/StepSimulationServiceHandler.cpp | 5 +++-- .../Source/Services/StepSimulationServiceHandler.h | 2 +- 31 files changed, 62 insertions(+), 57 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h index db4a75ac39..50b04055f3 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h @@ -13,10 +13,10 @@ #include #include #include - +#include namespace ROS2SimulationInterfaces { - using SimulationFeatureType = uint8_t; + using SimulationFeatureType = simulation_interfaces::msg::SimulatorFeatures::_features_type::value_type; class ROS2SimulationInterfacesRequests { public: diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h index 6a9de009db..6fc90a64a3 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h @@ -9,11 +9,12 @@ #pragma once #include #include +#include namespace SimulationInterfaces { //! Result codes to be used in the Result message //! @see Result.msg - using ErrorCodeType = uint8_t; + using ErrorCodeType = simulation_interfaces::msg::Result::_result_type; //! A message type to represent the result of a failed operation struct FailedResult diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h index d36648f259..6e6e126863 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/ROS2ActionBase.h @@ -93,7 +93,7 @@ namespace ROS2SimulationInterfaces //! 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 + AZStd::unordered_set GetProvidedFeatures() override { return {}; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp index 6fc01cba67..fca2f0b48e 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp @@ -23,11 +23,11 @@ namespace ROS2SimulationInterfaces SimulationInterfaces::SimulationManagerNotificationsBus::Handler::BusDisconnect(); } - AZStd::unordered_set SimulateStepsActionServerHandler::GetProvidedFeatures() + AZStd::unordered_set SimulateStepsActionServerHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_ACTION, - SimulationFeatures::STEP_SIMULATION_SINGLE, - SimulationFeatures::STEP_SIMULATION_MULTIPLE }; + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_ACTION, + SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE }; } AZStd::string_view SimulateStepsActionServerHandler::GetTypeName() const diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h index 1957444487..0bd6d9d4c9 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h @@ -25,7 +25,7 @@ namespace ROS2SimulationInterfaces ~SimulateStepsActionServerHandler(); // IROS2HandlerBase overrides - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::string_view GetTypeName() const override; AZStd::string_view GetDefaultName() const override; void Initialize(rclcpp::Node::SharedPtr& node) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp index f3256180ef..78f3930d87 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp @@ -103,9 +103,9 @@ namespace ROS2SimulationInterfaces } } - AZStd::unordered_set ROS2SimulationInterfacesSystemComponent::GetSimulationFeatures() + AZStd::unordered_set ROS2SimulationInterfacesSystemComponent::GetSimulationFeatures() { - AZStd::unordered_set result; + AZStd::unordered_set result; for (auto& [handlerType, handler] : m_availableRos2Interface) { auto features = handler->GetProvidedFeatures(); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h index 8e4720b127..5445869fcd 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h @@ -56,7 +56,7 @@ namespace ROS2SimulationInterfaces void Deactivate() override; // ROS2SimulationInterfacesRequestBus override - AZStd::unordered_set GetSimulationFeatures() override; + AZStd::unordered_set GetSimulationFeatures() override; private: AZStd::unordered_map> m_availableRos2Interface; diff --git a/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h b/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h index 51599db057..4f6d60b811 100644 --- a/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Interfaces/IROS2HandlerBase.h @@ -9,6 +9,7 @@ #pragma once #include #include +#include #include namespace ROS2SimulationInterfaces @@ -17,8 +18,9 @@ namespace ROS2SimulationInterfaces class IROS2HandlerBase { public: + using SimulationFeatureType = ROS2SimulationInterfaces::SimulationFeatureType; virtual ~IROS2HandlerBase() = default; - virtual AZStd::unordered_set GetProvidedFeatures() = 0; + 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; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp index c5b31a09c4..20e2a230cc 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -13,9 +13,9 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set DeleteEntityServiceHandler::GetProvidedFeatures() + AZStd::unordered_set DeleteEntityServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::DELETING }; + return AZStd::unordered_set{ SimulationFeatures::DELETING }; } AZStd::optional DeleteEntityServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h index a98f6d0a53..cd6e8f96a6 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "delete_entity"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp index ead483edc7..e37803a16f 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -13,12 +13,12 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetEntitiesServiceHandler::GetProvidedFeatures() + AZStd::unordered_set GetEntitiesServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, - SimulationFeatures::ENTITY_BOUNDS_BOX, - SimulationFeatures::ENTITY_BOUNDS_CONVEX, - SimulationFeatures::ENTITY_CATEGORIES }; + return AZStd::unordered_set{ SimulationFeatures::ENTITY_TAGS, + SimulationFeatures::ENTITY_BOUNDS_BOX, + SimulationFeatures::ENTITY_BOUNDS_CONVEX, + SimulationFeatures::ENTITY_CATEGORIES }; } AZStd::optional GetEntitiesServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h index a2f46ad4de..e5668fabed 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h @@ -26,7 +26,7 @@ namespace ROS2SimulationInterfaces { return "get_entities"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index 77cee8486a..2140a81f8c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -14,13 +14,13 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetEntitiesStatesServiceHandler::GetProvidedFeatures() + 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 }; + 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( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h index ca4228b486..8281548313 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "get_entities_states"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp index 0bf0588d8a..0633cca2da 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -14,9 +14,9 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetEntityStateServiceHandler::GetProvidedFeatures() + AZStd::unordered_set GetEntityStateServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_GETTING }; + return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_GETTING }; } AZStd::optional GetEntityStateServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h index 501109567a..5960c664c3 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "get_entity_state"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp index ab2066583e..b4f3510103 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.cpp @@ -13,9 +13,9 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetSimulationStateServiceHandler::GetProvidedFeatures() + AZStd::unordered_set GetSimulationStateServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_GETTING }; + return AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_GETTING }; } AZStd::optional GetSimulationStateServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h index a7cb5e4ebc..ff923a129d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationStateServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "get_simulation_state"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp index 2baba4a642..6435f30653 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -12,9 +12,9 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set GetSpawnablesServiceHandler::GetProvidedFeatures() + AZStd::unordered_set GetSpawnablesServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::SPAWNABLES }; + return AZStd::unordered_set{ SimulationFeatures::SPAWNABLES }; } AZStd::optional GetSpawnablesServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h index e2284ce247..0a14fe5838 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "get_spawnables"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h index b53f46c748..10259da1ea 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ROS2ServiceBase.h @@ -48,7 +48,7 @@ namespace ROS2SimulationInterfaces //! 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 + AZStd::unordered_set GetProvidedFeatures() override { return {}; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index e778126a97..165fa4da47 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -20,12 +20,12 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set ResetSimulationServiceHandler::GetProvidedFeatures() + AZStd::unordered_set ResetSimulationServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::SIMULATION_RESET, - SimulationFeatures::SIMULATION_RESET_TIME, - SimulationFeatures::SIMULATION_RESET_STATE, - SimulationFeatures::SIMULATION_RESET_SPAWNED }; + return AZStd::unordered_set{ SimulationFeatures::SIMULATION_RESET, + SimulationFeatures::SIMULATION_RESET_TIME, + SimulationFeatures::SIMULATION_RESET_STATE, + SimulationFeatures::SIMULATION_RESET_SPAWNED }; } AZStd::optional ResetSimulationServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h index b98995c4bd..c3de46c9f8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.h @@ -26,7 +26,7 @@ namespace ROS2SimulationInterfaces { return "reset_simulation"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp index 2064c5b92c..071237b396 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -13,9 +13,9 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set SetEntityStateServiceHandler::GetProvidedFeatures() + AZStd::unordered_set SetEntityStateServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_SETTING }; + return AZStd::unordered_set{ SimulationFeatures::ENTITY_STATE_SETTING }; } AZStd::optional SetEntityStateServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h index c120af23a2..4a144ecb9c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "set_entity_state"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp index 9461c236da..2e3b4edc67 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -13,9 +13,10 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set SetSimulationStateServiceHandler::GetProvidedFeatures() + AZStd::unordered_set SetSimulationStateServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_SETTING, SimulationFeatures::SIMULATION_STATE_PAUSE }; + return AZStd::unordered_set{ SimulationFeatures::SIMULATION_STATE_SETTING, + SimulationFeatures::SIMULATION_STATE_PAUSE }; } AZStd::optional SetSimulationStateServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h index c63bba3efd..23c55d8998 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.h @@ -27,7 +27,7 @@ namespace ROS2SimulationInterfaces { return "set_simulation_state"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; }; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp index b714eceb81..619bb8172d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -15,9 +15,9 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set SpawnEntityServiceHandler::GetProvidedFeatures() + AZStd::unordered_set SpawnEntityServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::SPAWNING }; + return AZStd::unordered_set{ SimulationFeatures::SPAWNING }; } AZStd::optional SpawnEntityServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h index f2021d02f2..e63ab0ff04 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h @@ -26,7 +26,7 @@ namespace ROS2SimulationInterfaces { return "spawn_entity"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp index 0291fcbb5a..fdc894e7eb 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.cpp @@ -14,9 +14,10 @@ namespace ROS2SimulationInterfaces { - AZStd::unordered_set StepSimulationServiceHandler::GetProvidedFeatures() + AZStd::unordered_set StepSimulationServiceHandler::GetProvidedFeatures() { - return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_SINGLE, SimulationFeatures::STEP_SIMULATION_MULTIPLE }; + return AZStd::unordered_set{ SimulationFeatures::STEP_SIMULATION_SINGLE, + SimulationFeatures::STEP_SIMULATION_MULTIPLE }; } AZStd::optional StepSimulationServiceHandler::HandleServiceRequest( diff --git a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h index 4899f27d48..a94494c146 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/StepSimulationServiceHandler.h @@ -29,7 +29,7 @@ namespace ROS2SimulationInterfaces { return "step_simulation"; } - AZStd::unordered_set GetProvidedFeatures() override; + AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; From 6384ec8cca7a16bb413fd28263c40f94e8fcc449 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 22 Apr 2025 11:32:55 +0200 Subject: [PATCH 32/35] Fix nitpicks in simulation interfaces (#893) Signed-off-by: Jan Hanca --- Gems/ROS2/Code/CMakeLists.txt | 1 - Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h | 7 +- Gems/SimulationInterfaces/Code/CMakeLists.txt | 74 +++++++++---------- .../ROS2SimulationInterfacesTypeIds.h | 2 +- .../Include/SimulationInterfaces/Result.h | 2 +- .../SimulationEntityManagerRequestBus.h | 12 +-- .../SimulationInterfacesTypeIds.h | 6 +- .../Include/SimulationInterfaces/TagFilter.h | 2 +- .../SimulateStepsActionServerHandler.cpp | 6 +- .../SimulateStepsActionServerHandler.h | 2 +- ...OS2SimulationInterfacesSystemComponent.cpp | 25 +++++-- .../ROS2SimulationInterfacesSystemComponent.h | 23 ++---- .../Clients/SimulationEntitiesManager.cpp | 42 +++++------ .../Clients/SimulationEntitiesManager.h | 1 - .../Clients/SimulationFeaturesAggregator.cpp | 4 - .../Clients/SimulationFeaturesAggregator.h | 3 - .../Code/Source/Clients/SimulationManager.cpp | 3 - .../Code/Source/Clients/SimulationManager.h | 1 - .../Services/DeleteEntityServiceHandler.h | 2 - .../Services/GetEntitiesServiceHandler.h | 2 - .../GetEntitiesStatesServiceHandler.h | 2 - .../Services/GetEntityStateServiceHandler.h | 2 - .../GetSimulationFeaturesServiceHandler.cpp | 8 +- .../GetSimulationFeaturesServiceHandler.h | 2 - .../Services/GetSpawnablesServiceHandler.cpp | 1 - .../Services/GetSpawnablesServiceHandler.h | 2 - .../ResetSimulationServiceHandler.cpp | 4 +- .../Services/SetEntityStateServiceHandler.h | 2 - .../Services/SpawnEntityServiceHandler.cpp | 12 +-- .../Services/SpawnEntityServiceHandler.h | 2 +- .../SimulationInterfacesModuleInterface.cpp | 4 +- .../Code/Source/Utils/Utils.h | 4 +- .../Clients/ROS2SimulationInterfacesTest.cpp | 11 --- .../Clients/SimulationInterfacesTest.cpp | 11 --- .../Code/Tests/Tools/InterfacesTest.cpp | 25 ++++++- Gems/SimulationInterfaces/gem.json | 14 ++-- 36 files changed, 142 insertions(+), 184 deletions(-) delete mode 100644 Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp delete mode 100644 Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index fe0f8f2b99..617b7c95e7 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -185,7 +185,6 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS) Gem::Atom_Feature_Common.Public ) - # By default, we will specify that the above target ROS2 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 or both of the following lines: diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h index bab1099d22..a4725e9d60 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h +++ b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h @@ -8,12 +8,12 @@ #pragma once #include "ITimeSource.h" +#include #include #include +#include #include #include -#include -#include namespace ROS2 { @@ -23,7 +23,6 @@ namespace ROS2 //! the /use_sim_time parameter set to true. class ROS2Clock { - public: ROS2Clock(); ROS2Clock(AZStd::unique_ptr timeSource, bool publishClock); @@ -37,7 +36,7 @@ namespace ROS2 //! 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; + 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. diff --git a/Gems/SimulationInterfaces/Code/CMakeLists.txt b/Gems/SimulationInterfaces/Code/CMakeLists.txt index e243943ba0..fdf8b33a5d 100644 --- a/Gems/SimulationInterfaces/Code/CMakeLists.txt +++ b/Gems/SimulationInterfaces/Code/CMakeLists.txt @@ -261,26 +261,26 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) ) ly_add_target( - NAME ${gem_name}.TestApp ${PAL_TRAIT_TEST_TARGET_TYPE} - NAMESPACE Gem - FILES_CMAKE + NAME ${gem_name}.TestApp ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE simulationinterfaces_editor_app_test.cmake - INCLUDE_DIRECTORIES + INCLUDE_DIRECTORIES PRIVATE - Tests - Source - Include - BUILD_DEPENDENCIES + 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 + 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 @@ -289,29 +289,29 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) ) ly_add_target( - NAME ${gem_name}.ROS2Tests ${PAL_TRAIT_TEST_TARGET_TYPE} - NAMESPACE Gem - FILES_CMAKE + NAME ${gem_name}.ROS2Tests ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE ros2_simulationinterfaces_tests_files.cmake - INCLUDE_DIRECTORIES + INCLUDE_DIRECTORIES PRIVATE - Tests - Source - Include - BUILD_DEPENDENCIES + 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 + 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 diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h index 34d94abd81..93240dde4c 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h @@ -22,5 +22,5 @@ namespace ROS2SimulationInterfaces inline constexpr const char* ROS2SimulationInterfacesEditorModuleTypeId = ROS2SimulationInterfacesModuleTypeId; // API TypeIds - inline constexpr const char* ROS2SimulationInterfacesRequestBusTypeId = "{00d08870-e329-4bd7-bb8c-f67fe369de92}"; + 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 index 6fc90a64a3..b95a70d6c1 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/Result.h @@ -13,7 +13,7 @@ namespace SimulationInterfaces { //! Result codes to be used in the Result message - //! @see Result.msg + //! @see Result.msg using ErrorCodeType = simulation_interfaces::msg::Result::_result_type; //! A message type to represent the result of a failed operation diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index bf2bca2feb..fb02ad1612 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -33,7 +33,7 @@ namespace SimulationInterfaces AZ::Transform m_boundsPose{ AZ::Transform::CreateIdentity() }; }; - //! @see EntityState.msg + //! @see EntityState.msg struct EntityState { AZ::Transform m_pose; //! The pose of the entity @@ -41,6 +41,7 @@ namespace SimulationInterfaces AZ::Vector3 m_twistAngular; //! The angular velocity of the entity (in the entity frame) }; + //! @see Spawnable.msg struct Spawnable { AZStd::string m_uri; @@ -64,16 +65,15 @@ namespace SimulationInterfaces //! 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 + //! @see GetEntities.srv virtual AZ::Outcome GetEntities(const EntityFilters& filter) = 0; //! Get the state of an entity. - //! @see GetEntityState.srv + //! @see GetEntityState.srv virtual AZ::Outcome GetEntityState(const AZStd::string& name) = 0; //! Get the state of all entities that match the filter. - //! @see GetEntitiesStates.srv + //! @see GetEntitiesStates.srv virtual AZ::Outcome GetEntitiesStates(const EntityFilters& filter) = 0; //! Set the state of an entity. @@ -92,7 +92,7 @@ namespace SimulationInterfaces 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 is empty, if the entity could not be registered (e.g. prefab has no simulated entities) + //! 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, diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h index bea5048d6c..9ab1568ef0 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationInterfacesTypeIds.h @@ -18,7 +18,7 @@ namespace SimulationInterfaces 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}"; + inline constexpr const char* SimulationFeaturesAggregatorEditorTypeId = "{504A86EF-DF0A-45EC-B69D-315FF4EC8121}"; // Module derived classes TypeIds inline constexpr const char* SimulationInterfacesModuleInterfaceTypeId = "{675797BF-E5D5-438A-BF86-4B4554F09CEF}"; @@ -30,7 +30,7 @@ namespace SimulationInterfaces // 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}"; + 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/TagFilter.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h index 1a1bb0e0bf..f9aa2c40ac 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/TagFilter.h @@ -14,7 +14,7 @@ namespace SimulationInterfaces { //! Structure to design a filter for tags - //! @see TagsFilter.msg + //! @see TagsFilter.msg using TagFilterMode = uint8_t; struct TagFilter diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp index fca2f0b48e..56caf70182 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.cpp @@ -57,7 +57,7 @@ namespace ROS2SimulationInterfaces { SimulationInterfaces::SimulationManagerRequestBus::Broadcast( &SimulationInterfaces::SimulationManagerRequests::CancelStepSimulation); - m_isCancelled = true; + m_cancelAction = true; return rclcpp_action::CancelResponse::ACCEPT; } @@ -80,7 +80,7 @@ namespace ROS2SimulationInterfaces m_goalHandle.reset(); return; } - m_isCancelled = false; + m_cancelAction = false; AZ::TickBus::Handler::BusConnect(); SimulationInterfaces::SimulationManagerRequestBus::Broadcast( &SimulationInterfaces::SimulationManagerRequests::StepSimulation, m_goalSteps); @@ -88,7 +88,7 @@ namespace ROS2SimulationInterfaces void SimulateStepsActionServerHandler::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) { - if (m_isCancelled) + if (m_cancelAction) { auto result = std::make_shared(); result->result.error_message = "Action has been cancelled."; diff --git a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h index 0bd6d9d4c9..af075c7e4b 100644 --- a/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Actions/SimulateStepsActionServerHandler.h @@ -44,7 +44,7 @@ namespace ROS2SimulationInterfaces private: AZ::u64 m_goalSteps{ 0 }; - bool m_isCancelled{ false }; + bool m_cancelAction{ false }; }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp index 78f3930d87..18cd236a36 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp @@ -9,15 +9,28 @@ #include "ROS2SimulationInterfacesSystemComponent.h" #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 ROS2SimulationInterfaces { @@ -67,10 +80,6 @@ namespace ROS2SimulationInterfaces { } - void ROS2SimulationInterfacesSystemComponent::Init() - { - } - void ROS2SimulationInterfacesSystemComponent::Activate() { ROS2SimulationInterfacesRequestBus::Handler::BusConnect(); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h index 5445869fcd..8db1b51f3a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h @@ -9,26 +9,14 @@ #pragma once #include -#include - -#include "Services/DeleteEntityServiceHandler.h" -#include "Services/GetEntitiesServiceHandler.h" -#include "Services/GetEntitiesStatesServiceHandler.h" -#include "Services/GetEntityStateServiceHandler.h" -#include "Services/GetSimulationFeaturesServiceHandler.h" -#include "Services/GetSimulationStateServiceHandler.h" -#include "Services/GetSpawnablesServiceHandler.h" -#include "Services/ROS2ServiceBase.h" -#include "Services/ResetSimulationServiceHandler.h" -#include "Services/SetEntityStateServiceHandler.h" -#include "Services/SetSimulationStateServiceHandler.h" -#include "Services/SpawnEntityServiceHandler.h" -#include "Services/StepSimulationServiceHandler.h" -#include "SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h" #include -#include #include #include +#include + +#include + +#include namespace ROS2SimulationInterfaces { @@ -51,7 +39,6 @@ namespace ROS2SimulationInterfaces protected: // AZ::Component interface implementation - void Init() override; void Activate() override; void Deactivate() override; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp index 0d886f7df1..221083ed84 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -8,12 +8,12 @@ #include "SimulationEntitiesManager.h" +#include +#include #include -#include "Clients/SimulationFeaturesAggregator.h" #include "CommonUtilities.h" #include "ConsoleCommands.inl" -#include "SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h" #include #include #include @@ -114,10 +114,6 @@ namespace SimulationInterfaces } } - void SimulationEntitiesManager::Init() - { - } - bool SimulationEntitiesManager::RegisterNewSimulatedBody(AzPhysics::SceneHandle sceneHandle, AzPhysics::SimulatedBodyHandle bodyHandle) { auto* scene = GetSceneHelper(sceneHandle); @@ -264,15 +260,16 @@ namespace SimulationInterfaces SimulationFeaturesAggregatorRequestBus::Broadcast( &SimulationFeaturesAggregatorRequests::AddSimulationFeatures, - AZStd::unordered_set{ // simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX, - // simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX, - // simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_GETTING, - simulation_interfaces::msg::SimulatorFeatures::ENTITY_STATE_SETTING, - simulation_interfaces::msg::SimulatorFeatures::DELETING, - simulation_interfaces::msg::SimulatorFeatures::SPAWNABLES, - simulation_interfaces::msg::SimulatorFeatures::SPAWNING }); + AZStd::unordered_set{ + // 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(); } @@ -287,7 +284,6 @@ namespace SimulationInterfaces m_physicsScenesHandle = AzPhysics::InvalidSceneHandle; m_sceneAddedHandler.Disconnect(); - m_sceneAddedHandler.Disconnect(); } AZStd::string SimulationEntitiesManager::AddSimulatedEntity(AZ::EntityId entityId, const AZStd::string& userProposedName) @@ -311,17 +307,15 @@ namespace SimulationInterfaces void SimulationEntitiesManager::RemoveSimulatedEntity(AZ::EntityId entityId) { - auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); - if (findIt != m_entityIdToSimulatedEntityMap.end()) + if (auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); findIt != m_entityIdToSimulatedEntityMap.end()) { const auto& simulatedEntityName = findIt->second; m_entityIdToSimulatedEntityMap.erase(findIt); m_simulatedEntityToEntityIdMap.erase(simulatedEntityName); } - auto findIt2 = m_entityIdToInitialState.find(entityId); - if (findIt2 != m_entityIdToInitialState.end()) + if (auto findIt = m_entityIdToInitialState.find(entityId); findIt != m_entityIdToInitialState.end()) { - m_entityIdToInitialState.erase(findIt2); + m_entityIdToInitialState.erase(findIt); } } @@ -371,8 +365,7 @@ namespace SimulationInterfaces for (const auto& hit : result.m_hits) { const AZ::EntityId entityId = hit.m_entityId; - auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); - if (findIt != m_entityIdToSimulatedEntityMap.end()) + if (auto findIt = m_entityIdToSimulatedEntityMap.find(entityId); findIt != m_entityIdToSimulatedEntityMap.end()) { entities.push_back(findIt->second); } @@ -404,7 +397,7 @@ namespace SimulationInterfaces const auto findIt = m_simulatedEntityToEntityIdMap.find(name); if (findIt == m_simulatedEntityToEntityIdMap.end()) { - AZ_Warning("SimulationInterfaces", findIt != m_simulatedEntityToEntityIdMap.end(), "Entity %s not found", name.c_str()); + 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{}; @@ -523,7 +516,6 @@ namespace SimulationInterfaces 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"))); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h index 7322467ae8..215845df66 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.h @@ -36,7 +36,6 @@ namespace SimulationInterfaces SimulationEntitiesManager(); ~SimulationEntitiesManager(); // AZ::Component interface implementation - void Init() override; void Activate() override; void Deactivate() override; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp index 699779c1ac..b7250f034c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.cpp @@ -60,10 +60,6 @@ namespace SimulationInterfaces } } - void SimulationFeaturesAggregator::Init() - { - } - void SimulationFeaturesAggregator::Activate() { SimulationFeaturesAggregatorRequestBus::Handler::BusConnect(); diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h index 9f445a1d48..45cc269eda 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationFeaturesAggregator.h @@ -14,9 +14,7 @@ #include #include #include -#include #include -#include namespace SimulationInterfaces { @@ -38,7 +36,6 @@ namespace SimulationInterfaces ~SimulationFeaturesAggregator(); // AZ::Component - void Init() override; void Activate() override; void Deactivate() override; diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index 2c941584c5..1c44c76ad5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -81,9 +81,6 @@ namespace SimulationInterfaces } } - void SimulationManager::Init() - { - } void SimulationManager::InitializeSimulationState() { // if start in stopped state, pause simulation. Default state for simulation by the standard is STOPPED and diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h index 7db04c7704..5e3c8acc00 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -42,7 +42,6 @@ namespace SimulationInterfaces ~SimulationManager(); // AZ::Component - void Init() override; void Activate() override; void Deactivate() override; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h index cd6e8f96a6..2f587a27fc 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.h @@ -30,8 +30,6 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h index e5668fabed..bea01df183 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.h @@ -29,8 +29,6 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h index 8281548313..02b52d7b9d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.h @@ -30,8 +30,6 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h index 5960c664c3..04855469c2 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.h @@ -30,7 +30,5 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 658150ebe5..165dfbea4c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -32,12 +32,12 @@ namespace ROS2SimulationInterfaces o3deInterfaces, &SimulationInterfaces::SimulationFeaturesAggregatorRequests::GetSimulationFeatures); // create common features and return it; // common features are logical AND between two sets - AZStd::unordered_set commonFeatures; - commonFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); - commonFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end()); + AZStd::unordered_set allFeatures; + allFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); + allFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end()); Response response; - for (auto id : commonFeatures) + for (auto id : allFeatures) { if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatureType(id))) { diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h index 8e993943b4..6c8c4fbab2 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.h @@ -31,8 +31,6 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp index 6435f30653..d22f120ec3 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -45,7 +45,6 @@ namespace ROS2SimulationInterfaces simSpawnable.description = spawnable.m_description.c_str(); return simSpawnable; }); - response.result.result = simulation_interfaces::msg::Result::RESULT_OK; return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h index 0a14fe5838..69159cc415 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.h @@ -30,8 +30,6 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index 165fa4da47..03490d437f 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -31,7 +31,7 @@ namespace ROS2SimulationInterfaces AZStd::optional ResetSimulationServiceHandler::HandleServiceRequest( const std::shared_ptr header, const Request& request) { - if (request.scope == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_STATE) + if (request.scope == Request::SCOPE_STATE) { Response response; SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast( @@ -40,7 +40,7 @@ namespace ROS2SimulationInterfaces return response; } - if (request.scope == simulation_interfaces::srv::ResetSimulation::Request::SCOPE_SPAWNED) + if (request.scope == Request::SCOPE_SPAWNED) { auto deletionCompletedCb = [this](const AZ::Outcome& outcome) { diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h index 4a144ecb9c..f096580556 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.h @@ -30,8 +30,6 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set GetProvidedFeatures() override; AZStd::optional HandleServiceRequest(const std::shared_ptr header, const Request& request) override; - - private: }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp index 619bb8172d..60544df56c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -37,8 +37,8 @@ namespace ROS2SimulationInterfaces return AZStd::nullopt; } - // Validate frame name - if (!entityNamespace.empty() && !ValidateFrameName(entityNamespace)) + // Validate namespace name + if (!entityNamespace.empty() && !ValidateNamespaceName(entityNamespace)) { Response response; response.result.result = simulation_interfaces::srv::SpawnEntity::Response::NAMESPACE_INVALID; @@ -81,12 +81,12 @@ namespace ROS2SimulationInterfaces return AZStd::regex_match(entityName, entityRegex); } - bool SpawnEntityServiceHandler::ValidateFrameName(const AZStd::string& frameName) + bool SpawnEntityServiceHandler::ValidateNamespaceName(const AZStd::string& namespaceName) { - const AZStd::regex frameRegex{ + const AZStd::regex namespaceRegex{ R"(^[a-zA-Z0-9_/]+$)" - }; // Entity names can only contain alphanumeric characters and underscores and forward slashes - return AZStd::regex_match(frameName, frameRegex); + }; // 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 index e63ab0ff04..26cba7d3ec 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.h @@ -32,7 +32,7 @@ namespace ROS2SimulationInterfaces private: bool ValidateEntityName(const AZStd::string& entityName); - bool ValidateFrameName(const AZStd::string& frameName); + bool ValidateNamespaceName(const AZStd::string& namespaceName); }; } // namespace ROS2SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp index 598d4a802b..61712def0d 100644 --- a/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp +++ b/Gems/SimulationInterfaces/Code/Source/SimulationInterfacesModuleInterface.cpp @@ -11,10 +11,10 @@ #include -#include "Clients/SimulationFeaturesAggregator.h" -#include "Clients/SimulationManager.h" #include #include +#include +#include namespace SimulationInterfaces { diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h index 57d7f2f8a8..d031a21d72 100644 --- a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h @@ -38,7 +38,7 @@ namespace ROS2SimulationInterfaces::Utils const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(lowerLeft, upperRight); filter.m_boundsShape = AZStd::make_shared(aabb.GetExtents()); } - else if (type == simulation_interfaces::msg::Bounds::TYPE_CONVEX_HULL) // TYPE_CONVEX_HULL + else if (type == simulation_interfaces::msg::Bounds::TYPE_CONVEX_HULL) { if (request.filters.bounds.points.size() < 3) { @@ -46,7 +46,7 @@ namespace ROS2SimulationInterfaces::Utils } filter.m_boundsShape = AZStd::make_shared(); } - else if (type == simulation_interfaces::msg::Bounds::TYPE_SPHERE) // TYPE_SPHERE + else if (type == simulation_interfaces::msg::Bounds::TYPE_SPHERE) { if (request.filters.bounds.points.size() != 2) { diff --git a/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp deleted file mode 100644 index 40217ff9bc..0000000000 --- a/Gems/SimulationInterfaces/Code/Tests/Clients/ROS2SimulationInterfacesTest.cpp +++ /dev/null @@ -1,11 +0,0 @@ -/* - * 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 - -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp deleted file mode 100644 index 40217ff9bc..0000000000 --- a/Gems/SimulationInterfaces/Code/Tests/Clients/SimulationInterfacesTest.cpp +++ /dev/null @@ -1,11 +0,0 @@ -/* - * 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 - -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp index 71a2b52c54..284fb1d90b 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -23,20 +23,37 @@ #include #include -#include "Clients/ROS2SimulationInterfacesSystemComponent.h" +#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 #include #include #include diff --git a/Gems/SimulationInterfaces/gem.json b/Gems/SimulationInterfaces/gem.json index f0316347e9..ff09497a45 100644 --- a/Gems/SimulationInterfaces/gem.json +++ b/Gems/SimulationInterfaces/gem.json @@ -7,22 +7,26 @@ "origin": "RobotecAI", "origin_url": "https://robotec.ai", "type": "Code", - "summary": "This gem provides C++ API for simulation interfaces.", + "summary": "This gem provides ROS 2 and C++ API for simulation interfaces.", "canonical_tags": [ "Gem" ], "user_tags": [ - "SimulationInterfaces", "ROS2", "ROS 2" + "SimulationInterfaces", + "ROS2", + "ROS 2" ], "platforms": [ "" ], "icon_path": "preview.png", - "requirements": "", + "requirements": "Requires ROS 2 Gem", "documentation_url": "", - "dependencies": ["ROS2"], + "dependencies": [ + "ROS2>=3.3.0" + ], "repo_uri": "", "compatible_engines": [], "engine_api_dependencies": [], "restricted": "SimulationInterfaces" -} +} \ No newline at end of file From 222d688dc9ff1e653ce3c4600dcd17271724b56d Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Mon, 28 Apr 2025 14:05:49 +0200 Subject: [PATCH 33/35] [simulation interfaces] Code review fixes (#896) * Code review fixes Signed-off-by: Jan Hanca * Fix FeaturesServiceHandler Signed-off-by: Jan Hanca --------- Signed-off-by: Jan Hanca --- .../Code/Include/ROS2/Clock/RealTimeSource.h | 10 +- .../SimulationEntityManagerRequestBus.h | 4 +- .../SimulationMangerRequestBus.h | 2 +- .../Code/Source/Clients/ConsoleCommands.inl | 261 ------------------ ...OS2SimulationInterfacesSystemComponent.cpp | 40 +-- .../ROS2SimulationInterfacesSystemComponent.h | 10 + .../Clients/SimulationEntitiesManager.cpp | 3 +- .../Services/DeleteEntityServiceHandler.cpp | 2 +- .../Services/GetEntitiesServiceHandler.cpp | 2 +- .../GetEntitiesStatesServiceHandler.cpp | 2 +- .../Services/GetEntityStateServiceHandler.cpp | 2 +- .../GetSimulationFeaturesServiceHandler.cpp | 13 +- .../Services/GetSpawnablesServiceHandler.cpp | 2 +- .../ResetSimulationServiceHandler.cpp | 2 +- .../Services/SetEntityStateServiceHandler.cpp | 2 +- .../SetSimulationStateServiceHandler.cpp | 2 +- .../Services/SpawnEntityServiceHandler.cpp | 2 +- .../SimulationInterfacesEditorModule.cpp | 5 +- .../Code/Source/Utils/RegistryUtils.cpp | 2 +- .../Code/Source/Utils/RegistryUtils.h | 2 +- .../Code/Source/Utils/Utils.h | 10 +- .../Code/simulationinterfaces_api_files.cmake | 1 + 22 files changed, 59 insertions(+), 322 deletions(-) delete mode 100644 Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h b/Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h index a976c9f3fa..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,10 +22,12 @@ 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. diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h index fb02ad1612..08514e8ba3 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationEntityManagerRequestBus.h @@ -72,8 +72,10 @@ namespace SimulationInterfaces //! @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. @@ -84,7 +86,7 @@ namespace SimulationInterfaces //! @see DeleteEntity.srv virtual void DeleteEntity(const AZStd::string& name, DeletionCompletedCb completedCb) = 0; - //! Remove all previously spawned entity from the simulation. + //! Remove all previously spawned entities from the simulation. virtual void DeleteAllEntities(DeletionCompletedCb completedCb) = 0; //! Get a list of spawnable entities. diff --git a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h index 2f683e1e64..deb5c7629c 100644 --- a/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h +++ b/Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationMangerRequestBus.h @@ -29,7 +29,7 @@ namespace SimulationInterfaces using ReloadLevelCallback = AZStd::function; - //! Reload level and removal of all spawned simulation entities. + //! Reload level and remove all spawned simulation entities. virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0; //! Set the simulation to paused or unpaused, diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl b/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl deleted file mode 100644 index 38dd7d7984..0000000000 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl +++ /dev/null @@ -1,261 +0,0 @@ -/* - * 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 - -namespace SimulationInterfacesCommands -{ - - using namespace SimulationInterfaces; - static void simulationinterfaces_GetEntities(const AZ::ConsoleCommandContainer& arguments) - { - AZ::Outcome entities; - SimulationEntityManagerRequestBus::BroadcastResult( - entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters()); - if (!entities.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str()); - return; - } - - for (const auto& entity : entities.GetValue()) - { - AZ_Printf("SimulationInterfacesConsole", " - %s\n", entity.c_str()); - } - } - - static void simulationinterfaces_Pause(const AZ::ConsoleCommandContainer& arguments) - { - SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::SetSimulationPaused, true); - } - - static void simulationinterfaces_Resume(const AZ::ConsoleCommandContainer& arguments) - { - SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::SetSimulationPaused, false); - } - - static void simulationinterfaces_Step(const AZ::ConsoleCommandContainer& arguments) - { - if (arguments.empty()) - { - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_Step \n"); - return; - } - uint32_t steps = AZStd::stoi(AZStd::string(arguments[0])); - - SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::StepSimulation, steps); - } - - static void simulationinterfaces_GetEntitiesSphere(const AZ::ConsoleCommandContainer& arguments) - { - float sphereShape = 10.f; - AZ::Vector3 position = AZ::Vector3::CreateZero(); - sphereShape = arguments.empty() ? 10.f : (AZStd::stof(AZStd::string(arguments[0]))); - position.SetX(arguments.size() > 1 ? (AZStd::stof(AZStd::string(arguments[1]))) : 0.f); - position.SetY(arguments.size() > 2 ? (AZStd::stof(AZStd::string(arguments[2]))) : 0.f); - position.SetZ(arguments.size() > 3 ? (AZStd::stof(AZStd::string(arguments[3]))) : 0.f); - - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntities in radius %f \n", sphereShape); - AZ_Printf("SimulationInterfacesConsole", "position %f %f %f \n", position.GetX(), position.GetY(), position.GetZ()); - EntityFilters filter; - filter.m_boundsShape = AZStd::make_shared(sphereShape); - - AZ::Outcome entities; - SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter); - if (!entities.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str()); - return; - } - - for (const auto& entity : entities.GetValue()) - { - AZ_Printf("SimulationInterfacesConsole", " - %s\n", entity.c_str()); - } - } - - static void simulationinterfaces_GetEntityState(const AZ::ConsoleCommandContainer& arguments) - { - if (arguments.empty()) - { - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState requires entity name\n"); - return; - } - const AZStd::string entityName = arguments[0]; - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState %s\n", entityName.c_str()); - AZ::Outcome entityStateResult; - SimulationEntityManagerRequestBus::BroadcastResult( - entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName); - if (!entityStateResult.IsSuccess()) - { - AZ_Printf( - "SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().m_errorString.c_str()); - return; - } - const auto& entityState = entityStateResult.GetValue(); - AZ_Printf("SimulationInterfacesConsole", "Entity %s\n", entityName.c_str()); - AZ_Printf("SimulationInterfacesConsole", "Pose %s\n", AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str()); - AZ_Printf("SimulationInterfacesConsole", "Rotation %s \n", AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str()); - - const AZ::Vector3 euler = entityState.m_pose.GetRotation().GetEulerDegrees(); - AZ_Printf("SimulationInterfacesConsole", "Rotation (euler) %s\n", AZ::Vector3ToString(euler).c_str()); - AZ_Printf("SimulationInterfacesConsole", "Twist Linear %s\n", AZ::Vector3ToString(entityState.m_twistLinear).c_str()); - AZ_Printf("SimulationInterfacesConsole", "Twist Angular %s\n", AZ::Vector3ToString(entityState.m_twistAngular).c_str()); - } - - static void simulationinterfaces_SetStateXYZ(const AZ::ConsoleCommandContainer& arguments) - { - if (arguments.empty()) - { - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState requires entity name\n"); - return; - } - const AZStd::string entityName = arguments[0]; - AZ::Vector3 position = AZ::Vector3::CreateZero(); - position.SetX(arguments.size() > 1 ? (AZStd::stof(AZStd::string(arguments[1]))) : 0.f); - position.SetY(arguments.size() > 2 ? (AZStd::stof(AZStd::string(arguments[2]))) : 0.f); - position.SetZ(arguments.size() > 3 ? (AZStd::stof(AZStd::string(arguments[3]))) : 0.f); - EntityState entityState{}; - entityState.m_pose = AZ::Transform::CreateIdentity(); - entityState.m_pose.SetTranslation(position); - AZ::Outcome result; - SimulationEntityManagerRequestBus::BroadcastResult( - result, &SimulationEntityManagerRequestBus::Events::SetEntityState, entityName, entityState); - - if (!result.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().m_errorString.c_str()); - return; - } - AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str()); - } - - static void simulationinterfaces_DeleteEntity(const AZ::ConsoleCommandContainer& arguments) - { - if (arguments.empty()) - { - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_DeleteEntity requires entity name\n"); - return; - } - const AZStd::string entityName = arguments[0]; - DeletionCompletedCb cb = [](const AZ::Outcome& result) - { - if (result.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "Entity deleted\n"); - } - else - { - AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().m_errorString.c_str()); - } - }; - SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, cb); - } - - static void simulationinterfaces_GetSpawnables(const AZ::ConsoleCommandContainer& arguments) - { - AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetSpawnables\n"); - AZ::Outcome spawnables; - SimulationEntityManagerRequestBus::BroadcastResult(spawnables, &SimulationEntityManagerRequestBus::Events::GetSpawnables); - if (!spawnables.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().m_errorString.c_str()); - return; - } - for (const auto& spawnable : spawnables.GetValue()) - { - AZ_Printf("SimulationInterfaces", " - %s\n", spawnable.m_uri.c_str()); - } - } - - static void simulationinterfaces_Spawn(const AZ::ConsoleCommandContainer& arguments) - { - if (arguments.size() < 2) - { - AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn minimal :\n"); - AZ_Printf("SimulationInterfacesConsole", " simulationinterface_Spawn \n"); - AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn optional :\n"); - AZ_Printf("SimulationInterfacesConsole", " simulationinterface_Spawn \n"); - return; - } - AZStd::string name = arguments[0]; - AZStd::string uri = arguments[1]; - AZStd::string entityNamespace = arguments.size() > 2 ? arguments[2] : ""; - AZ::Transform initialPose = AZ::Transform::CreateIdentity(); - if (arguments.size() > 5) - { - initialPose.SetTranslation(AZ::Vector3( - AZStd::stof(AZStd::string(arguments[3])), - AZStd::stof(AZStd::string(arguments[4])), - AZStd::stof(AZStd::string(arguments[5])))); - } - SpawnCompletedCb completedCb = [](const AZ::Outcome& result) - { - if (!result.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().m_errorString.c_str()); - return; - } - AZ_Printf("SimulationInterfacesConsole", "Entity spawned and registered : %s\n", result.GetValue().c_str()); - }; - constexpr bool allowRename = true; - SimulationEntityManagerRequestBus::Broadcast( - &SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb); - AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str()); - } - - static void simulationinterfaces_ReloadLevel(const AZ::ConsoleCommandContainer& arguments) - { - SimulationManagerRequests::ReloadLevelCallback cb = []() - { - AZ_Printf("SimulationInterfacesConsole", "Reload level completed\n"); - }; - SimulationManagerRequestBus::Broadcast(&SimulationManagerRequestBus::Events::ReloadLevel, cb); - } - - static void simulationinterfaces_DeleteAll(const AZ::ConsoleCommandContainer& arguments) - { - DeletionCompletedCb cb = [](const AZ::Outcome& result) - { - if (result.IsSuccess()) - { - AZ_Printf("SimulationInterfacesConsole", "All spawned entities deleted\n"); - } - else - { - AZ_Printf( - "SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().m_errorString.c_str()); - } - }; - SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, cb); - } - - AZ_CONSOLEFREEFUNC( - simulationinterfaces_GetEntities, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the scene."); - - AZ_CONSOLEFREEFUNC(simulationinterfaces_Pause, AZ::ConsoleFunctorFlags::DontReplicate, "Pause simulation."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_Resume, AZ::ConsoleFunctorFlags::DontReplicate, "Resume simulation."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_Step, AZ::ConsoleFunctorFlags::DontReplicate, "Step simulation."); - - AZ_CONSOLEFREEFUNC( - simulationinterfaces_GetEntitiesSphere, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the radius."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_GetEntityState, AZ::ConsoleFunctorFlags::DontReplicate, "Get state of the entity."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_SetStateXYZ, AZ::ConsoleFunctorFlags::DontReplicate, "Set state of the entity."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_DeleteEntity, AZ::ConsoleFunctorFlags::DontReplicate, "Delete entity."); - AZ_CONSOLEFREEFUNC( - simulationinterfaces_GetSpawnables, AZ::ConsoleFunctorFlags::DontReplicate, "Get all spawnable entities in the scene."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_Spawn, AZ::ConsoleFunctorFlags::DontReplicate, "Spawn entity."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_ReloadLevel, AZ::ConsoleFunctorFlags::DontReplicate, "Reload level."); - AZ_CONSOLEFREEFUNC(simulationinterfaces_DeleteAll, AZ::ConsoleFunctorFlags::DontReplicate, "Remove all spawned entities."); - -} // namespace SimulationInterfacesCommands diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp index 18cd236a36..ad31a1e9e3 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.cpp @@ -34,20 +34,6 @@ namespace ROS2SimulationInterfaces { - - namespace - { - template - void RegisterInterface( - AZStd::unordered_map>& interfacesMap, rclcpp::Node::SharedPtr ros2Node) - { - AZStd::shared_ptr handler = AZStd::make_shared(); - handler->Initialize(ros2Node); - interfacesMap[handler->GetTypeName()] = AZStd::move(handler); - handler.reset(); - }; - } // namespace - AZ_COMPONENT_IMPL( ROS2SimulationInterfacesSystemComponent, "ROS2SimulationInterfacesSystemComponent", ROS2SimulationInterfacesSystemComponentTypeId); @@ -87,19 +73,19 @@ namespace ROS2SimulationInterfaces rclcpp::Node::SharedPtr ros2Node = rclcpp::Node::SharedPtr(ROS2::ROS2Interface::Get()->GetNode()); AZ_Assert(ros2Node, "ROS2 node is not available."); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, ros2Node); - RegisterInterface(m_availableRos2Interface, 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); + RegisterInterface(ros2Node); } void ROS2SimulationInterfacesSystemComponent::Deactivate() diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h index 8db1b51f3a..199ca7d163 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/ROS2SimulationInterfacesSystemComponent.h @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -47,6 +48,15 @@ namespace ROS2SimulationInterfaces 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 index 221083ed84..9b48df0c59 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp @@ -13,7 +13,6 @@ #include #include "CommonUtilities.h" -#include "ConsoleCommands.inl" #include #include #include @@ -466,9 +465,9 @@ namespace SimulationInterfaces void SimulationEntitiesManager::SetEntitiesState(const AZStd::vector& entityAndDescendants, const EntityState& state) { - AZ_Assert(!entityAndDescendants.empty(), "Entity and descendants list is empty"); if (entityAndDescendants.empty()) { + AZ_Error("SimulationInterfaces", false, "Entity and descendants list is empty"); return; } const AZ::EntityId entityId = entityAndDescendants.front(); diff --git a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp index 20e2a230cc..13ea23d75d 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/DeleteEntityServiceHandler.cpp @@ -36,7 +36,7 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp index e37803a16f..d1654e0eae 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesServiceHandler.cpp @@ -43,7 +43,7 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp index 2140a81f8c..c28d3ed771 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntitiesStatesServiceHandler.cpp @@ -45,7 +45,7 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp index 0633cca2da..423fb1be27 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetEntityStateServiceHandler.cpp @@ -33,7 +33,7 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp index 165dfbea4c..e4376c8fe8 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp @@ -9,6 +9,7 @@ #include "GetSimulationFeaturesServiceHandler.h" #include #include +#include #include namespace ROS2SimulationInterfaces @@ -30,21 +31,19 @@ namespace ROS2SimulationInterfaces AZStd::unordered_set o3deInterfaces; SimulationInterfaces::SimulationFeaturesAggregatorRequestBus::BroadcastResult( o3deInterfaces, &SimulationInterfaces::SimulationFeaturesAggregatorRequests::GetSimulationFeatures); + // create common features and return it; // common features are logical AND between two sets - AZStd::unordered_set allFeatures; - allFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end()); - allFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end()); - Response response; - for (auto id : allFeatures) + for (auto id : o3deInterfaces) { - if (ros2Interfaces.contains(id) && o3deInterfaces.contains(SimulationInterfaces::SimulationFeatureType(id))) + 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/GetSpawnablesServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp index d22f120ec3..d2f2e87de4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/GetSpawnablesServiceHandler.cpp @@ -28,7 +28,7 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); return response; } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp index 03490d437f..7f488c2c8c 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/ResetSimulationServiceHandler.cpp @@ -52,7 +52,7 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp index 071237b396..f701ad60d1 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetEntityStateServiceHandler.cpp @@ -36,7 +36,7 @@ namespace ROS2SimulationInterfaces if (!outcome.IsSuccess()) { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); } diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp index 2e3b4edc67..b713a420e5 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SetSimulationStateServiceHandler.cpp @@ -34,7 +34,7 @@ namespace ROS2SimulationInterfaces } else { - response.result.result = static_cast(transitionResult.GetError().m_errorCode); + response.result.result = transitionResult.GetError().m_errorCode; response.result.error_message = transitionResult.GetError().m_errorString.c_str(); } return response; diff --git a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp index 60544df56c..a9cef9f4f4 100644 --- a/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Services/SpawnEntityServiceHandler.cpp @@ -67,7 +67,7 @@ namespace ROS2SimulationInterfaces else { const auto& failedResult = outcome.GetError(); - response.result.result = aznumeric_cast(failedResult.m_errorCode); + response.result.result = failedResult.m_errorCode; response.result.error_message = failedResult.m_errorString.c_str(); } SendResponse(response); diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp index b82b9126b5..1f308adbde 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationInterfacesEditorModule.cpp @@ -32,14 +32,11 @@ namespace SimulationInterfaces }); } - /** - * Add required SystemComponents to the SystemEntity. - * Non-SystemComponents should not be added here - */ AZ::ComponentTypeList GetRequiredSystemComponents() const override { return AZ::ComponentTypeList{ azrtti_typeid(), + azrtti_typeid(), azrtti_typeid(), azrtti_typeid(), }; diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp index 83dec3826b..7164a6db32 100644 --- a/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.cpp @@ -12,7 +12,7 @@ namespace ROS2SimulationInterfaces::RegistryUtilities { - AZStd::string GetName(AZStd::string serviceType) + AZStd::string GetName(const AZStd::string& serviceType) { AZ::SettingsRegistryInterface* settingsRegistry = AZ::SettingsRegistry::Get(); AZ_Assert(settingsRegistry, "Settings Registry is not available"); diff --git a/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h index 2bedd7f224..85e2c58288 100644 --- a/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/RegistryUtils.h @@ -16,5 +16,5 @@ namespace ROS2SimulationInterfaces::RegistryUtilities //! Gets name of the service with defined type form settings registry //! @return string with service name, if setting registry doesn't exits returns empty string - [[nodiscard]] AZStd::string GetName(AZStd::string serviceType); + [[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 index d031a21d72..0a544f0148 100644 --- a/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h +++ b/Gems/SimulationInterfaces/Code/Source/Utils/Utils.h @@ -33,9 +33,9 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid points! The first point should be lower than the second point."); } - const auto upperRight = ROS2::ROS2Conversions::FromROS2Vector3(p2); - const auto lowerLeft = ROS2::ROS2Conversions::FromROS2Vector3(p1); - const AZ::Aabb aabb = AZ::Aabb::CreateFromMinMax(lowerLeft, upperRight); + 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) @@ -44,7 +44,9 @@ namespace ROS2SimulationInterfaces::Utils { return AZ::Failure("Invalid number of points! Type 'TYPE_CONVEX_HULL' should have at least 3 points."); } - filter.m_boundsShape = AZStd::make_shared(); + // 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) { diff --git a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake index ae0596f11a..c87b6436a1 100644 --- a/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake +++ b/Gems/SimulationInterfaces/Code/simulationinterfaces_api_files.cmake @@ -12,4 +12,5 @@ set(FILES Include/SimulationInterfaces/SimulationMangerRequestBus.h Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h Include/SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h + Include/SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h ) From 3d448aa7949152a833624884bb4e3eb77aba7495 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 29 Apr 2025 17:02:14 +0200 Subject: [PATCH 34/35] Added SpinUntilFuture to test fixture (#899) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added SpinUntilFuture to test fixture Signed-off-by: Michał Pełka Co-authored-by: Jan Hanca --- .../Code/Tests/Tools/InterfacesTest.cpp | 58 ++++++++++++------- 1 file changed, 37 insertions(+), 21 deletions(-) diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp index 284fb1d90b..592655d267 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -105,16 +105,32 @@ namespace UnitTest AZ_Assert(node, "Node is not available."); return node; } - void SpinAppSome() + 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 < 10; ++i) + 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 @@ -186,7 +202,7 @@ namespace UnitTest })); auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -217,7 +233,7 @@ namespace UnitTest })); auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -265,7 +281,7 @@ namespace UnitTest })); auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -284,7 +300,7 @@ namespace UnitTest request->filters.bounds.type = simulation_interfaces::msg::Bounds::TYPE_SPHERE; auto future = client->async_send_request(request); - SpinAppSome(); + 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); @@ -332,7 +348,7 @@ namespace UnitTest })); auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -361,7 +377,7 @@ namespace UnitTest request->filters.bounds.type = simulation_interfaces::msg::Bounds::TYPE_BOX; auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -377,7 +393,7 @@ namespace UnitTest auto client = node->create_client("/get_simulation_features"); auto request = std::make_shared(); auto future = client->async_send_request(request); - SpinAppSome(); + 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."; @@ -405,7 +421,7 @@ namespace UnitTest auto client = node->create_client("/get_simulation_features"); auto request = std::make_shared(); auto future = client->async_send_request(request); - SpinAppSome(); + 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."; @@ -443,7 +459,7 @@ namespace UnitTest EXPECT_TRUE(allowRename); completedCb(AZ::Success("valid_name")); })); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -464,7 +480,7 @@ namespace UnitTest request->entity_namespace = "test_namespace"; auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -486,7 +502,7 @@ namespace UnitTest request->entity_namespace = "invalid namespace"; auto future = client->async_send_request(request); - SpinAppSome(); + SpinAppUntilFuture(future); ASSERT_TRUE(future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Service call timed out."; auto response = future.get(); @@ -523,12 +539,12 @@ namespace UnitTest })); auto future = client->async_send_goal(*goal); - SpinAppSome(); + 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); - SpinAppSome(); + 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); } @@ -552,12 +568,12 @@ namespace UnitTest })); auto future = client->async_send_goal(*goal); - SpinAppSome(); + 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); - SpinAppSome(); + 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); } @@ -593,13 +609,13 @@ namespace UnitTest EXPECT_CALL(mock, CancelStepSimulation()); auto future = client->async_send_goal(*goal); - SpinAppSome(); + 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 cancelFeautre = client->async_cancel_goal(goalHandle); - SpinAppSome(); - ASSERT_TRUE(cancelFeautre.wait_for(std::chrono::seconds(0)) == std::future_status::ready) << "Action call timed out."; + 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 From 8cf97e31a34d08dfb0dd321918ffeb9a15c28dbf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 30 Apr 2025 15:42:45 +0200 Subject: [PATCH 35/35] Disable certain state transitions in Editor (#901) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Disable certain state transitions in Editor * Add optional debug draw for StateName. * Add registry settings to Registry directory. Signed-off-by: Michał Pełka Co-authored-by: Jan Hanca --- Gems/SimulationInterfaces/Code/CMakeLists.txt | 1 + .../Code/Source/Clients/SimulationManager.cpp | 76 ++++++++++++++++++- .../Code/Source/Clients/SimulationManager.h | 7 ++ .../Source/Tools/SimulationManagerEditor.cpp | 8 +- .../Source/Tools/SimulationManagerEditor.h | 5 ++ .../Code/Tests/Tools/InterfacesTest.cpp | 3 +- .../simulationinterface_settings.setreg | 6 ++ Gems/SimulationInterfaces/gem.json | 3 +- 8 files changed, 105 insertions(+), 4 deletions(-) create mode 100644 Gems/SimulationInterfaces/Registry/simulationinterface_settings.setreg diff --git a/Gems/SimulationInterfaces/Code/CMakeLists.txt b/Gems/SimulationInterfaces/Code/CMakeLists.txt index fdf8b33a5d..2ec9280728 100644 --- a/Gems/SimulationInterfaces/Code/CMakeLists.txt +++ b/Gems/SimulationInterfaces/Code/CMakeLists.txt @@ -57,6 +57,7 @@ ly_add_target( 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) diff --git a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp index 1c44c76ad5..108b2bf099 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -22,8 +23,27 @@ 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(); @@ -32,6 +52,15 @@ namespace SimulationInterfaces 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); @@ -63,6 +92,7 @@ namespace SimulationInterfaces 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() @@ -112,6 +142,10 @@ namespace SimulationInterfaces 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]() { @@ -121,6 +155,7 @@ namespace SimulationInterfaces void SimulationManager::Deactivate() { + AZ::TickBus::Handler::BusDisconnect(); SimulationManagerRequestBus::Handler::BusDisconnect(); } @@ -246,11 +281,24 @@ namespace SimulationInterfaces "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 (%d -> %d) is forbidden", m_simulationState, stateToSet))); + AZStd::string::format("Requested transition (%s -> %s) is forbidden", currentStateName.c_str(), stateToSetName.c_str()))); } switch (stateToSet) @@ -299,6 +347,23 @@ namespace SimulationInterfaces 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 }; @@ -306,4 +371,13 @@ namespace SimulationInterfaces 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 index 5e3c8acc00..99cc15239e 100644 --- a/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h +++ b/Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.h @@ -27,6 +27,7 @@ namespace SimulationInterfaces : public AZ::Component , protected SimulationManagerRequestBus::Handler , protected AzFramework::LevelSystemLifecycleNotificationBus::Handler + , protected AZ::TickBus::Handler { public: AZ_COMPONENT_DECL(SimulationManager); @@ -59,7 +60,11 @@ namespace SimulationInterfaces // 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; @@ -68,6 +73,8 @@ namespace SimulationInterfaces }; // 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{ { diff --git a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp index a99fb4f4c5..98c40fd2f7 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.cpp @@ -57,14 +57,20 @@ namespace SimulationInterfaces void SimulationManagerEditor::Activate() { - BaseSystemComponent::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 index 86b7ab3b70..538a59c97a 100644 --- a/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h +++ b/Gems/SimulationInterfaces/Code/Source/Tools/SimulationManagerEditor.h @@ -9,6 +9,7 @@ #pragma once #include +#include #include @@ -18,6 +19,7 @@ namespace SimulationInterfaces class SimulationManagerEditor : public SimulationManager , protected AzToolsFramework::EditorEvents::Bus::Handler + , private AzToolsFramework::EditorEntityContextNotificationBus::Handler { using BaseSystemComponent = SimulationManager; @@ -39,5 +41,8 @@ namespace SimulationInterfaces void Init() override; void Activate() override; void Deactivate() override; + + // EditorEntityContextNotificationBus + void OnStartPlayInEditorBegin() override; }; } // namespace SimulationInterfaces diff --git a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp index 592655d267..c34504a122 100644 --- a/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp +++ b/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp @@ -116,7 +116,8 @@ namespace UnitTest } } - template void SpinAppUntilFuture(Future &future) + template + void SpinAppUntilFuture(Future& future) { AZ::ComponentApplication* app = nullptr; AZ::ComponentApplicationBus::BroadcastResult(app, &AZ::ComponentApplicationBus::Events::GetApplication); 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 index ff09497a45..48ea321ff0 100644 --- a/Gems/SimulationInterfaces/gem.json +++ b/Gems/SimulationInterfaces/gem.json @@ -23,7 +23,8 @@ "requirements": "Requires ROS 2 Gem", "documentation_url": "", "dependencies": [ - "ROS2>=3.3.0" + "ROS2>=3.3.0", + "DebugDraw" ], "repo_uri": "", "compatible_engines": [],