Преглед изворни кода

Removed copied simulation_interfaces msgs (#868)

Signed-off-by: Norbert Prokopiuk <[email protected]>
Norbert Prokopiuk пре 3 месеци
родитељ
комит
353a245fb7

+ 0 - 47
Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeatures.h

@@ -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 <AzCore/base.h>
-
-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

+ 2 - 2
Gems/SimulationInterfaces/Code/Include/SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h

@@ -10,13 +10,13 @@
 
 #include <SimulationInterfaces/SimulationInterfacesTypeIds.h>
 
-#include "SimulationFeatures.h"
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Interface/Interface.h>
 #include <AzCore/std/containers/unordered_set.h>
 
 namespace SimulationInterfaces
 {
+    using SimulationFeatures = uint8_t;
 
     class SimulationFeaturesAggregatorRequests
     {
@@ -32,7 +32,7 @@ namespace SimulationInterfaces
         virtual const AZStd::unordered_set<SimulationFeatures> 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;
     };
 

+ 21 - 20
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 <AzCore/Asset/AssetManager.h>
 #include <AzCore/Asset/AssetManagerBus.h>
@@ -30,9 +29,10 @@
 #include <AzFramework/Spawnable/Spawnable.h>
 #include <AzFramework/Spawnable/SpawnableEntitiesInterface.h>
 
+#include <ROS2/Frame/ROS2FrameComponent.h>
 #include <simulation_interfaces/msg/result.hpp>
+#include <simulation_interfaces/msg/simulator_features.hpp>
 #include <simulation_interfaces/srv/spawn_entity.hpp>
-#include <ROS2/Frame/ROS2FrameComponent.h>
 namespace SimulationInterfaces
 {
     void SetRigidBodyVelocities(AzPhysics::RigidBody* rigidBody, const EntityState& state)
@@ -211,15 +211,15 @@ namespace SimulationInterfaces
 
         SimulationFeaturesAggregatorRequestBus::Broadcast(
             &SimulationFeaturesAggregatorRequests::AddSimulationFeatures,
-            AZStd::unordered_set<SimulationFeatures>{ 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<SimulationFeatures>{ simulation_interfaces::msg::SimulatorFeatures::ENTITY_TAGS,
+                                                      simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_BOX,
+                                                      simulation_interfaces::msg::SimulatorFeatures::ENTITY_BOUNDS_CONVEX,
+                                                      simulation_interfaces::msg::SimulatorFeatures::ENTITY_CATEGORIES,
+                                                      simulation_interfaces::msg::SimulatorFeatures::ENTITY_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)
             {

+ 11 - 12
Gems/SimulationInterfaces/Code/Source/Clients/SimulationManager.cpp

@@ -7,7 +7,6 @@
  */
 
 #include "SimulationManager.h"
-#include "SimulationInterfaces/SimulationFeatures.h"
 
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Serialization/SerializeContext.h>
@@ -15,6 +14,7 @@
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <SimulationInterfaces/SimulationFeaturesAggregatorRequestBus.h>
 #include <SimulationInterfaces/SimulationInterfacesTypeIds.h>
+#include <simulation_interfaces/msg/simulator_features.hpp>
 
 namespace SimulationInterfaces
 {
@@ -76,15 +76,14 @@ namespace SimulationInterfaces
         SimulationManagerRequestBus::Handler::BusConnect();
         SimulationFeaturesAggregatorRequestBus::Broadcast(
             &SimulationFeaturesAggregatorRequests::AddSimulationFeatures,
-            AZStd::unordered_set<SimulationFeatures>{
-                SimulationFeatures::SIMULATION_RESET,
-                SimulationFeatures::SIMULATION_RESET_TIME,
-                //SimulationFeatures::SIMULATION_RESET_STATE,
-                SimulationFeatures::SIMULATION_RESET_SPAWNED,
-                SimulationFeatures::SIMULATION_STATE_PAUSE,
-                SimulationFeatures::STEP_SIMULATION_SINGLE,
-                SimulationFeatures::STEP_SIMULATION_MULTIPLE,
-                SimulationFeatures::STEP_SIMULATION_ACTION});
+            AZStd::unordered_set<SimulationFeatures>{ 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);

+ 0 - 1
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
 )

+ 2 - 4
Gems/SimulationInterfacesROS2/Code/Source/Services/GetSimulationFeaturesServiceHandler.cpp

@@ -35,10 +35,8 @@ namespace SimulationInterfacesROS2
         // common features are logical AND between two sets
         AZStd::unordered_set<AZ::u8> commonFeatures;
         commonFeatures.insert(ros2Interfaces.begin(), ros2Interfaces.end());
-        for (auto id : o3deInterfaces)
-        {
-            commonFeatures.insert(static_cast<AZ::u8>(id));
-        }
+        commonFeatures.insert(o3deInterfaces.begin(), o3deInterfaces.end());
+
         Response response;
         for (auto id : commonFeatures)
         {

+ 12 - 13
Gems/SimulationInterfacesROS2/Code/Tests/Tools/InterfacesTest.cpp

@@ -25,17 +25,18 @@
 
 #include "Clients/SimulationInterfacesROS2SystemComponent.h"
 #include "Mocks/SimulationEntityManagerMock.h"
+#include "Mocks/SimulationFeaturesAggregatorRequestsHandlerMock.h"
 #include <QApplication>
 #include <ROS2/ROS2Bus.h>
 #include <gtest/gtest.h>
 #include <memory>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <simulation_interfaces/msg/simulator_features.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <string>
 #include <string_view>
 #include <vector>
-#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<simulation_interfaces::srv::DeleteEntity::Request>();
         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<simulation_interfaces::srv::DeleteEntity::Request>();
         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<Physics::SphereShapeConfiguration*>(filter.m_bounds_shape.get());
+                        Physics::SphereShapeConfiguration* sphereShape =
+                            azdynamic_cast<Physics::SphereShapeConfiguration*>(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<Physics::BoxShapeConfiguration*>(filter.m_bounds_shape.get());
+                        Physics::BoxShapeConfiguration* boxShape =
+                            azdynamic_cast<Physics::BoxShapeConfiguration*>(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<SimulationFeatures> features {
-            SimulationFeatures::SPAWNING,
+        AZStd::unordered_set<SimulationFeatures> features{
+            simulation_interfaces::msg::SimulatorFeatures::SPAWNING,
             static_cast<SimulationFeatures>(0xFE), // invalid feature, should be ignored
             static_cast<SimulationFeatures>(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