Add callback to spawn entity service (#611)

Signed-off-by: Artur Kamieniecki <[email protected]>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp

@@ -40,9 +40,12 @@ namespace ROS2
 
         m_spawnService = ros2Node->create_service<gazebo_msgs::srv::SpawnEntity>(
             "spawn_entity",
-            [this](const SpawnEntityRequest request, SpawnEntityResponse response)
+            [this](
+                const SpawnEntityServiceHandle service_handle,
+                const std::shared_ptr<rmw_request_id_t> header,
+                const SpawnEntityRequest request)
             {
-                SpawnEntity(request, response);
+                SpawnEntity(service_handle, header, request);
             });
 
         m_getSpawnPointInfoService = ros2Node->create_service<gazebo_msgs::srv::GetModelState>(
@@ -89,17 +92,21 @@ namespace ROS2
         }
     }
 
-    void ROS2SpawnerComponent::SpawnEntity(const SpawnEntityRequest request, SpawnEntityResponse response)
+    void ROS2SpawnerComponent::SpawnEntity(
+        const SpawnEntityServiceHandle service_handle, const std::shared_ptr<rmw_request_id_t> header, const SpawnEntityRequest request)
     {
         AZStd::string spawnableName(request->name.c_str());
         AZStd::string spawnableNamespace(request->robot_namespace.c_str());
         AZStd::string spawnPointName(request->xml.c_str(), request->xml.size());
 
+        SpawnEntityResponse response;
+
         auto namespaceValidation = ROS2Names::ValidateNamespace(spawnableNamespace);
         if (!namespaceValidation.IsSuccess())
         {
-            response->success = false;
-            response->status_message = namespaceValidation.GetError().data();
+            response.success = false;
+            response.status_message = namespaceValidation.GetError().data();
+            service_handle->send_response(*header, response);
             return;
         }
 
@@ -107,8 +114,9 @@ namespace ROS2
 
         if (!m_controller.GetSpawnables().contains(spawnableName))
         {
-            response->success = false;
-            response->status_message = "Could not find spawnable with given name: " + request->name;
+            response.success = false;
+            response.status_message = "Could not find spawnable with given name: " + request->name;
+            service_handle->send_response(*header, response);
             return;
         }
 
@@ -146,9 +154,14 @@ namespace ROS2
             PreSpawn(id, view, transform, spawnableName, spawnableNamespace);
         };
 
-        spawner->SpawnAllEntities(m_tickets.at(spawnableName), optionalArgs);
+        optionalArgs.m_completionCallback = [service_handle, header](auto id, auto view)
+        {
+            SpawnEntityResponse response;
+            response.success = true;
+            service_handle->send_response(*header, response);
+        };
 
-        response->success = true;
+        spawner->SpawnAllEntities(m_tickets.at(spawnableName), optionalArgs);
     }
 
     void ROS2SpawnerComponent::PreSpawn(

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h

@@ -26,7 +26,8 @@ namespace ROS2
     using GetAvailableSpawnableNamesRequest = std::shared_ptr<gazebo_msgs::srv::GetWorldProperties::Request>;
     using GetAvailableSpawnableNamesResponse = std::shared_ptr<gazebo_msgs::srv::GetWorldProperties::Response>;
     using SpawnEntityRequest = std::shared_ptr<gazebo_msgs::srv::SpawnEntity::Request>;
-    using SpawnEntityResponse = std::shared_ptr<gazebo_msgs::srv::SpawnEntity::Response>;
+    using SpawnEntityResponse = gazebo_msgs::srv::SpawnEntity::Response;
+    using SpawnEntityServiceHandle = std::shared_ptr<rclcpp::Service<gazebo_msgs::srv::SpawnEntity>>;
     using GetSpawnPointInfoRequest = std::shared_ptr<gazebo_msgs::srv::GetModelState::Request>;
     using GetSpawnPointInfoResponse = std::shared_ptr<gazebo_msgs::srv::GetModelState::Response>;
     using GetSpawnPointsNamesRequest = std::shared_ptr<gazebo_msgs::srv::GetWorldProperties::Request>;
@@ -61,7 +62,11 @@ namespace ROS2
         rclcpp::Service<gazebo_msgs::srv::GetModelState>::SharedPtr m_getSpawnPointInfoService;
 
         void GetAvailableSpawnableNames(const GetAvailableSpawnableNamesRequest request, GetAvailableSpawnableNamesResponse response);
-        void SpawnEntity(const SpawnEntityRequest request, SpawnEntityResponse response);
+        void SpawnEntity(
+            const SpawnEntityServiceHandle service_handle,
+            const std::shared_ptr<rmw_request_id_t> header,
+            const SpawnEntityRequest request);
+
         void PreSpawn(
             AzFramework::EntitySpawnTicket::Id,
             AzFramework::SpawnableEntityContainerView,