feat: Populate scene using batch spawning

pixi.toml

@@ -146,6 +146,7 @@ description = "Clone o3de-extras gems locally and validate all gem paths from pr
 # No cwd override: the script path is project-root-relative and the script cd's
 # into $DEMO_ROOT/sim itself.
 cmd = "bash scripts/build_sim.sh"
+inputs = ["scripts/build_sim.sh", "sim/Gem/Source/*", "sim/Gem/Include/*", "sim/Gem/CMakeLists.txt", "sim/Gem/gem.json", "sim/Gem/*.cmake"]
 outputs = ["sim/build/linux/bin/profile/MobileManipulatorDemo.GameLauncher"]
 # All gems are registered project-locally via external_subdirectories in
 # sim/project.json, so the gem source trees must exist before cmake configure.

rai_app/environment/scene_manager.py

@@ -33,11 +33,14 @@
 )
 from rosidl_runtime_py.convert import message_to_ordereddict
 from simulation_interfaces.msg import EntityState
+from simulation_interfaces.msg import SpawnEntity as SpawnEntityMsg
 from simulation_interfaces.srv import (
     GetEntities,
     GetEntityState,
     SetEntityState,
+    SpawnEntities,
     SpawnEntity,
+    SpawnEntity_Request,
 )
 from tf2_geometry_msgs import do_transform_pose
 from tf_transformations import euler_from_quaternion, quaternion_from_euler
@@ -192,12 +195,8 @@ def populate_scene(
                 "Slots and object names must have the same length and items stored"
             )
 
-        simulation_names: list[str] = []
-        for slot, object_name, item in tqdm(
-            zip(slots, object_names, items_stored),
-            desc="Spawning entities",
-            total=len(slots),
-        ):
+        reqs: list[SpawnEntityMsg] = []
+        for slot, object_name, item in zip(slots, object_names, items_stored):
             if np.isclose(offset_yaw, 0.0):
                 should_rotate = random.random() < percent_of_rotated_objects
                 if should_rotate:
@@ -206,36 +205,35 @@ def populate_scene(
                 else:
                     offset_yaw = 0.0
 
-            simulation_name = self.spawn_on_spot(
+            req = self.make_spawn_entity_msg_for_spot(
                 slot_name=slot,
                 object_name=object_name,
                 item_stored=item,
                 std_xy=std_xy,
                 std_yaw=std_yaw,
                 offset_yaw=offset_yaw,
             )
-            self.logger.info(f"Simulation name: {simulation_name}")
-            simulation_names.append(simulation_name)
+            reqs.append(req)
+
+        simulation_names: list[str] = self.spawn_objects(reqs)
         return simulation_names
 
-    def spawn_object(
+    def init_spawn_entity_name_and_pose(
         self,
+        req: SpawnEntityMsg | SpawnEntity_Request,
         pose: Pose,
         object_name: str,
         item_stored: Optional[str] = None,
         frame: str = "odom",
     ):
-        wait_for_ros2_services(self.connector, ["/spawn_entity"])
         # NOTE (jmatejcz) item stored will be added to name of object
         # and that's how it will be distinguished
         if item_stored:
             name = object_name + f"__{item_stored}__" + str(uuid.uuid4())[:8]
         else:
             name = object_name + str(uuid.uuid4())[:8]
 
-        req = SpawnEntity.Request()
         req.name = name
-        req.uri = self.spawnable_to_uri[object_name]
         req.initial_pose.header.frame_id = frame
         req.initial_pose.pose.position.x = pose.position.x
         req.initial_pose.pose.position.y = pose.position.y
@@ -245,7 +243,34 @@ def spawn_object(
         req.initial_pose.pose.orientation.z = pose.orientation.z
         req.initial_pose.pose.orientation.w = pose.orientation.w
 
-        self.logger.debug(f"Spawning {name}")
+        return req
+
+    def make_spawn_entity_msg(
+        self,
+        pose: Pose,
+        object_name: str,
+        item_stored: Optional[str] = None,
+        frame: str = "odom",
+    ) -> SpawnEntityMsg:
+        req = SpawnEntityMsg()
+        self.init_spawn_entity_name_and_pose(req, pose, object_name, item_stored, frame)
+        req.entity_resource.uri = self.spawnable_to_uri[object_name]
+        return req
+
+    def spawn_object(
+        self,
+        pose: Pose,
+        object_name: str,
+        item_stored: Optional[str] = None,
+        frame: str = "odom",
+    ):
+        wait_for_ros2_services(self.connector, ["/spawn_entity"])
+
+        req = SpawnEntity.Request()
+        self.init_spawn_entity_name_and_pose(req, pose, object_name, item_stored, frame)
+        req.uri = self.spawnable_to_uri[object_name]
+
+        self.logger.debug(f"Spawning {req.name}")
         result = self.connector.call_service(
             ROS2Message(payload=message_to_ordereddict(req)),
             target="/spawn_entity",
@@ -254,9 +279,29 @@ def spawn_object(
             reuse_client=True,
         ).payload
         result = cast(SpawnEntity.Response, result)
-        return name
+        return req.name
+
+    def spawn_objects(self, requests: list[SpawnEntityMsg]):
+        wait_for_ros2_services(self.connector, ["/spawn_entities"])
+
+        req = SpawnEntities.Request()
+        req.spawn_requests = requests
 
-    def spawn_on_spot(
+        self.logger.debug(f"Spawning {[request.name for request in requests]}")
+        result = self.connector.call_service(
+            ROS2Message(payload=message_to_ordereddict(req)),
+            target="/spawn_entities",
+            msg_type="simulation_interfaces/srv/SpawnEntities",
+            timeout_sec=3.0,
+            reuse_client=True,
+        ).payload
+        result = cast(SpawnEntities.Response, result)
+        for res in result.results:
+            if res.result.result != 1:
+                print(f"ERROR: {res.result.error_message}")
+        return [res.entity_name for res in result.results]
+
+    def make_spawn_entity_msg_for_spot(
         self,
         slot_name: str,
         object_name: str,
@@ -265,8 +310,7 @@ def spawn_on_spot(
         std_yaw: float = 0.0,
         offset_yaw: float = 0.0,
         frame: str = "odom",
-    ):
-        wait_for_ros2_services(self.connector, ["/spawn_entity"])
+    ) -> SpawnEntityMsg:
         pose: Pose = copy.deepcopy(self.slots[slot_name].origin_pose)
         # Add Gaussian noise to x, y
         pose.position.x += random.normalvariate(0, std_xy)
@@ -287,9 +331,7 @@ def spawn_on_spot(
         pose.orientation.z = q_new[2]
         pose.orientation.w = q_new[3]
 
-        return self.spawn_object(
-            pose=pose, object_name=object_name, item_stored=item_stored, frame=frame
-        )
+        return self.make_spawn_entity_msg(pose, object_name, item_stored, frame)
 
     def clear_scene(self):
         wait_for_ros2_services(self.connector, ["/get_entities", "/delete_entity"])

sim/Gem/MobileManipulatorDemo_files.cmake

@@ -8,6 +8,8 @@ set(FILES
 
     Source/SpawnEntityServiceHandler.cpp
     Source/SpawnEntityServiceHandler.h
+    Source/SpawnEntitiesServiceHandler.cpp
+    Source/SpawnEntitiesServiceHandler.h
     Source/SpawnServiceUtils.cpp
     Source/SpawnServiceUtils.h
 )

sim/Gem/Source/MobileManipulatorDemoSystemComponent.cpp

@@ -7,6 +7,7 @@
 
 #include <ROS2/ROS2Bus.h>
 #include "SpawnEntityServiceHandler.h"
+#include "SpawnEntitiesServiceHandler.h"
 
 namespace MobileManipulatorDemo
 {
@@ -82,6 +83,7 @@ namespace MobileManipulatorDemo
         }
 
         RegisterInterface<SpawnEntityServiceHandler>(ros2Node);
+        RegisterInterface<SpawnEntitiesServiceHandler>(ros2Node);
     }
 
     void MobileManipulatorDemoSystemComponent::DestroyHandlers()

sim/Gem/Source/SpawnEntitiesServiceHandler.cpp

@@ -0,0 +1,179 @@
+// NOTE: this file is a slightly modified copy of SimulationInterfaces/Code/Source/Services/SpawnEntitiesServiceHandler.cpp
+
+/*
+ * 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 "SpawnEntitiesServiceHandler.h"
+#include "SpawnServiceUtils.h"
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/TF/TransformInterface.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
+#include <SimulationInterfaces/RegistryUtils.h>
+#include <SimulationInterfaces/SimulationEntityManagerRequestBus.h>
+#include <SimulationInterfaces/ROS2SimulationInterfacesRequestBus.h>
+
+namespace MobileManipulatorDemo
+{
+    SpawnEntitiesServiceHandler::SpawnEntitiesServiceHandler()
+    {
+        ROS2SimulationInterfaces::ROS2SimulationInterfacesRequestBus::Broadcast(
+            &ROS2SimulationInterfaces::ROS2SimulationInterfacesRequests::AddSimulationFeatures,
+            AZStd::unordered_set<ROS2SimulationInterfaces::SimulationFeatureType>{
+                simulation_interfaces::msg::SimulatorFeatures::SPAWNING_ENTITIES });
+    }
+
+    AZStd::optional<SpawnEntitiesServiceHandler::Response> SpawnEntitiesServiceHandler::HandleServiceRequest(
+        const std::shared_ptr<rmw_request_id_t> header, const Request& request)
+    {
+        const builtin_interfaces::msg::Time zeroTime = builtin_interfaces::msg::Time();
+        const auto simulatorFrameId = ROS2SimulationInterfaces::RegistryUtilities::GetSimulatorROS2Frame();
+
+        Response response;
+        response.results.resize(request.spawn_requests.size());
+
+        AZStd::vector<SimulationInterfaces::SpawningEntity> spawningEntities;
+        spawningEntities.reserve(request.spawn_requests.size());
+        AZStd::vector<size_t> spawnedRequestsIndices;
+        spawnedRequestsIndices.reserve(request.spawn_requests.size());
+
+        bool hasFailures = false;
+        for (size_t requestIdx = 0; requestIdx < request.spawn_requests.size(); ++requestIdx)
+        {
+            const auto& spawnRequest = request.spawn_requests[requestIdx];
+            auto& spawnResult = response.results[requestIdx];
+
+            const AZStd::string_view name{ spawnRequest.name.c_str(), spawnRequest.name.size() };
+            const AZStd::string_view uri{ spawnRequest.entity_resource.uri.c_str(), spawnRequest.entity_resource.uri.size() };
+            const AZStd::string_view entityNamespace{ spawnRequest.entity_namespace.c_str(), spawnRequest.entity_namespace.size() };
+            const AZStd::string_view messageFrameId{ spawnRequest.initial_pose.header.frame_id.c_str(),
+                                                     spawnRequest.initial_pose.header.frame_id.size() };
+
+            if (!name.empty() && !SpawnServiceUtils::ValidateEntityName(name))
+            {
+                spawnResult.result.result = simulation_interfaces::msg::SpawnResult::NAME_INVALID;
+                spawnResult.result.error_message =
+                    "Invalid entity name. Entity names can only contain alphanumeric characters and underscores.";
+                hasFailures = true;
+                continue;
+            }
+
+            if (!entityNamespace.empty() && !SpawnServiceUtils::ValidateNamespaceName(entityNamespace))
+            {
+                spawnResult.result.result = simulation_interfaces::msg::SpawnResult::NAMESPACE_INVALID;
+                spawnResult.result.error_message =
+                    "Invalid entity namespace. Entity namespaces can only contain alphanumeric characters and forward slashes.";
+                hasFailures = true;
+                continue;
+            }
+
+            AZ::Transform transformOffset = AZ::Transform::CreateIdentity();
+            if (!messageFrameId.empty() && simulatorFrameId != messageFrameId)
+            {
+                auto transformInterface = ROS2::TFInterface::Get();
+                AZ_Assert(transformInterface, "TFInterface is not available, cannot set entity state without transform offset.");
+                const auto transformOutcome = transformInterface->GetTransform(simulatorFrameId, messageFrameId, zeroTime);
+
+                if (transformOutcome.IsSuccess())
+                {
+                    transformOffset = transformOutcome.GetValue();
+                }
+                else
+                {
+                    spawnResult.result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
+                    spawnResult.result.error_message = transformOutcome.GetError().c_str();
+                    hasFailures = true;
+                    continue;
+                }
+            }
+
+            const AZ::Transform requestedPose = ROS2::ROS2Conversions::FromROS2Pose(spawnRequest.initial_pose.pose);
+            if (const auto poseValidation = SpawnServiceUtils::ValidateTransformNormalized(requestedPose); !poseValidation.IsSuccess())
+            {
+                spawnResult.result.result = simulation_interfaces::msg::SpawnResult::INVALID_POSE;
+                spawnResult.result.error_message = poseValidation.GetError().c_str();
+                hasFailures = true;
+                continue;
+            }
+
+            const AZ::Transform initialPose = transformOffset *
+                AZ::Transform::CreateFromQuaternionAndTranslation(
+                                                  requestedPose.GetRotation().GetNormalized(), requestedPose.GetTranslation());
+
+            SimulationInterfaces::SpawningEntity spawningEntity;
+            spawningEntity.name = AZStd::string(name);
+            spawningEntity.uri = AZStd::string(uri);
+            spawningEntity.entityNamespace = AZStd::string(entityNamespace);
+            spawningEntity.initialPose = initialPose;
+            spawningEntity.allowRename = spawnRequest.allow_renaming;
+            spawningEntity.preinsertionCb =
+                [](const AZ::Outcome<AzFramework::SpawnableEntityContainerView, SimulationInterfaces::FailedResult>&)
+            {
+            };
+            spawningEntity.completedCb = [](const AZ::Outcome<AZStd::string, SimulationInterfaces::FailedResult>&)
+            {
+            };
+
+            spawningEntities.push_back(AZStd::move(spawningEntity));
+            spawnedRequestsIndices.push_back(requestIdx);
+        }
+
+        if (spawningEntities.empty())
+        {
+            response.result.result = hasFailures ? simulation_interfaces::srv::SpawnEntities::Response::ENTITIES_SPAWN_FAILED
+                                                 : simulation_interfaces::msg::Result::RESULT_OK;
+            if (hasFailures)
+            {
+                response.result.error_message = "One or more entity spawn requests failed.";
+            }
+            SendResponse(response);
+            return AZStd::nullopt;
+        }
+
+        SimulationInterfaces::SimulationEntityManagerRequestBus::Broadcast(
+            &SimulationInterfaces::SimulationEntityManagerRequests::SpawnEntities,
+            spawningEntities,
+            [this, response, spawnedRequestsIndices, hasFailures](const SimulationInterfaces::BatchSpawnResult& batchResult) mutable
+            {
+                bool hasBatchFailures = hasFailures;
+
+                for (size_t spawnedIdx = 0; spawnedIdx < batchResult.m_spawnResults.size() && spawnedIdx < spawnedRequestsIndices.size();
+                     ++spawnedIdx)
+                {
+                    const size_t requestIdx = spawnedRequestsIndices[spawnedIdx];
+                    auto& spawnResult = response.results[requestIdx];
+                    const auto& outcome = batchResult.m_spawnResults[spawnedIdx];
+
+                    if (outcome.IsSuccess())
+                    {
+                        spawnResult.result.result = simulation_interfaces::msg::Result::RESULT_OK;
+                        spawnResult.entity_name = outcome.GetValue().c_str();
+                        SpawnServiceUtils::RegisterChildGrippingPoints(outcome.GetValue());
+                    }
+                    else
+                    {
+                        const auto& failedResult = outcome.GetError();
+                        spawnResult.result.result = failedResult.m_errorCode;
+                        spawnResult.result.error_message = failedResult.m_errorString.c_str();
+                        hasBatchFailures = true;
+                    }
+                }
+
+                response.result.result = hasBatchFailures ? simulation_interfaces::srv::SpawnEntities::Response::ENTITIES_SPAWN_FAILED
+                                                          : simulation_interfaces::msg::Result::RESULT_OK;
+                if (hasBatchFailures)
+                {
+                    response.result.error_message = "One or more entity spawn requests failed.";
+                }
+
+                SendResponse(response);
+            });
+
+        return AZStd::nullopt;
+    }
+
+} // namespace MobileManipulatorDemo